A flexible joint motion speed tracking system for large inertia variable load in space

Abstract

The present invention discloses a flexible joint motion speed tracking system for large inertia variable load in space, comprising: a joint position detection unit, a position loop controller, a speed loop controller, a current loop controller and a motor drive module. The output of the position loop controller is selected as compensation, and the output is summed with the desired speed as the desired input of the speed loop controller. The present invention introduces a high-frequency oscillation suppression function in proportional control to suppress speed oscillation, increases a negative feedback damping link, and improves the adaptability of the controlled object to large-scale changes in load inertia.

Description

Technical Field

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

[0002] Load inertia is the load characteristic generated by the rotational inertia of spatial 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, the load inertia ratio needs to be reasonably designed to avoid motion oscillation and other situations.

[0003] Compared with the ground gravity environment, the microgravity environment in 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 ground mechanisms by a large margin, which leads to large oscillations during the motion process of space mechanisms, especially during acceleration and deceleration. At the same time, the load inertia of space multi-body systems represented by space manipulators changes all the time during motion, and due to the existence of joint flexibility and nonlinear factors, the difficulty of motion control of large inertia variable load joints in space is greatly increased. The control methods for flexible joints of robots mainly include traditional PD control and singular perturbation, cascade system, intelligent control and other methods. Among them, PD control is simple, but often cannot meet the control accuracy requirements; other methods need to be combined with adaptive control or robust control to solve the problem of decreased control performance caused by uncertainty of robot parameters, resulting in many control methods. The implementation process is quite complicated, and most of them are still in the theoretical research or simulation stage, and are less used in engineering. Summary of the invention

[0004] The technical problem solved by the present invention is: to overcome the deficiencies of the prior art and provide a flexible joint motion speed tracking system suitable for flexible joints with large inertia and variable load, so as to meet the requirements of motion stability and positioning accuracy of spatial flexible joints.

[0005] The technical solution of the present invention is:

[0006] A flexible joint motion speed tracking system for space large inertia variable load, comprising: a joint position detection unit, a position loop controller, a speed loop controller, a current loop controller and a motor drive module;

[0007] Position loop controller: According to the desired joint angle and the actual joint angle collected by the joint position detection unit, the position loop control cycle T P Determine the compensation speed V conp (m) and transmitted to the speed loop controller;

[0008] Speed ​​loop controller: receives the compensation speed V transmitted by the position loop controller conp (m), the expected joint velocity V receiving the input cmd (k), according to the compensation speed V conp (m) According to the speed loop control period T v Expected joint velocity V cmd (k) Compensate and obtain the expected speed V after compensation cmdIn (k); According to the expected speed V after compensation cmdIn (k) and the actual angular velocity V of the motor fed back by the motor drive module m (k), determine the expected output current i Qcmd (k) and transmitted to the current loop controller;

[0009] Current loop controller: receives the output desired current i transmitted by the speed loop controller Qcmd (k), receiving the actual current i corresponding to the motor output voltage fed back by the motor drive module Q (k), according to the expected output current i Qcmd (k) and the actual current i Q (k) Adjust the drive voltage output to the motor drive module so that the actual current i Q (k) Continuously output the expected current i Qcmd (k) conduct tracking;

[0010] 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 of the motor m (k) to the speed loop controller; feedback the actual current i corresponding to the motor output voltage Q (k) To the current loop controller.

[0011] Preferably, the position loop controller determines the compensation speed V conp (m), specifically:

[0012] According to the joint expected angle θ of the current mth position loop control cycle cmd (m) and the actual joint angle θ j (m), determine the angle error θ of the current mth position loop control cycle err (m);

[0013] According to the actual joint angle θ of the current mth position loop control cycle j (m) and the actual joint angle θ of the m-1th position loop control cycle j (m-1), determine the differential value of the actual joint angle of the current mth position loop control cycle

[0014] According to the angle error θ err (m) and the differential value of the actual angle of the joint Determine the compensation speed V conp (m).

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

[0016] θ err (m) = θ cmd (m)-θ j (m).

[0017] Preferably, the position loop controller determines the differential value of the actual joint angle of the current mth position loop control cycle Specifically:

[0018]

[0019] Preferably, the position loop controller determines the compensation speed V conp (m), specifically:

[0020]

[0021] Among them, K P-P is the proportional coefficient of the position loop controller, K P-D is the differential coefficient of the position loop controller.

[0022] Preferably, the speed loop controller controls the joint desired speed V cmd (k) Compensate and obtain the expected speed V after compensation cmdIn (k) specifically:

[0023] V cmdIn (k) = V cmd (k)+V conp (m)

[0024] Among them, in the position loop control period T P and speed loop control period T v When the ratio Q is 1, m=k;

[0025] When Q is a positive integer greater than 1:

[0026]

[0027] Wherein, n is a positive integer.

[0028] Preferably, the speed loop controller is based on the compensated desired speed V cmdIn (k) and the actual angular velocity V of the motor m(k), determine the expected output current i Qcmd (k) specifically:

[0029] The speed loop controller obtains the desired speed V after compensation. cmdIn (k) and the actual angular velocity V of the motor m (k), obtain 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 the motor speed damping V m_F (k);

[0030] 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), determine the expected output current i Qcmd (k).

[0031] Preferably, the speed loop controller obtains the suppressed motor angular velocity error V m_errPF The details are as follows:

[0032]

[0033] V m_err (k) = V m_cmd (k)-V m (k)

[0034] V m_cmd (k) = V cmdIn (k)×N

[0035] Among them, T Pf is the oscillation suppression coefficient, T Pf Less than 1, N is the joint transmission ratio;

[0036] The speed loop controller obtains the speed error integral value V m_errI (k) are as follows:

[0037]

[0038] Preferably, the speed loop controller obtains the motor angular velocity differential value V m_D (k) are as follows:

[0039]

[0040] V m_err (k) = V m_cmd (k)-Vm (k)

[0041] V m_cmd (k) = V cmdIn (k)×N.

[0042] Wherein, N is the joint transmission ratio;

[0043] The speed loop controller obtains the motor speed damping V m_F (k) are as follows:

[0044] V m_F (k) = K V-F ×V m (k)

[0045] Among them, K V-F is the damping coefficient of the speed loop controller.

[0046] Preferably, the speed loop controller determines the output desired current i Qcmd (k) specifically:

[0047] i Qcmd (k) = K V-P ×V m_errPF (k)+K V-I ×V m_errI (k)+K V-D ×V m_D (k)-V m_F (k)

[0048] Among them, K V-P is the proportional coefficient of the speed loop controller, K V-I is the integral coefficient of the speed loop controller, K V-D is the differential coefficient of the speed loop controller.

[0049] The advantages of the present invention compared with the prior art are:

[0050] The present invention can realize stable speed control and motion precision control of flexible mechanisms such as joints under the conditions of large load inertia and variable load in space, can effectively suppress speed oscillation when the joints are started and stopped, and adapt to a wide range of changes in load inertia. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 Motion control system architecture for flexible joints with large inertia and variable load;

[0052] Figure 2 Block diagram of space flexible joint velocity control. DETAILED DESCRIPTION

[0053] The present invention provides a flexible joint motion speed tracking system for large inertia variable load in space, which uses the basic principle of PID controller and proposes the following according to the characteristics of space flexible joint motion control: Figure 1 The motion control architecture shown in the figure sums the output of the position loop controller with the desired speed in a compensated form as the input information of the speed loop; introduces a high-frequency oscillation suppression function in the speed closed loop proportional term; and adds negative feedback damping control in the speed closed loop. Figure 2 The speed control method shown converts the desired speed of the flexible joint into the desired speed of the motor, and after subtracting it from the actual speed of the motor, inputs the speed loop proportional control, speed loop integral control, and speed loop differential control respectively, wherein the speed loop proportional control is designed with a high-frequency oscillation suppression function. The speed loop damping link is designed to achieve speed negative feedback by introducing the actual speed of the motor. After summing (differentiation) all control links, the desired current is output for current control. The present invention can be applied to the motion control of space flexible joints with large inertia and variable load characteristics.

[0054] Load inertia is the load characteristic generated by the rotational inertia of joints and other mechanisms applied to the motor shaft. On the ground, the ratio of the load inertia of joints using harmonic, planetary and other reducers to the inertia of the motor is usually less than 10. In space applications, considering the microgravity environment of space, the ratio of the load inertia of the joint to the inertia of the motor will reach 10,000 or even higher, and for multi-body systems represented by robots, the load inertia of the joints changes continuously with the change of configuration.

[0055] According to the requirements of large inertia and variable load motion control of space flexible joints, the present invention controls the speed loop period T v Expected joint velocity V cmd (k) Compensate and obtain the expected speed V after compensation cmdIn (k), described as:

[0056] V cmdIn (k) = V cmd (k)+V conp (m)

[0057] Among them, V cmd (k) is the desired joint velocity, V conp (m) is the compensation speed calculated by the position loop controller. The control period of the position loop controller is T P Should not be less than the control period T of the speed loop controller v , and T P T v Q times, Q is a positive integer. In the position loop control period T P and speed loop control period T v When the ratio Q is 1, m=k;

[0058] When Q is a positive integer greater than 1:

[0059]

[0060] Wherein, n is a positive integer.

[0061] The position loop controller of the mth position loop control cycle is described as:

[0062]

[0063] K P-P is the proportional coefficient of the position loop controller, K P-D These are the differential coefficients of the position loop controller. The two coefficients need to be debugged. The initial values ​​can be set to 1 and 0, and then debugged. During the debugging, the response curve of the position loop is obtained, and the two coefficients are adjusted according to the response conditions until the response time, stabilization time, stability margin and other indicators are met.

[0064] θ err (m) is the angle error, θ err (m) = θ cmd (m)-θ j (m), θ cmd (m) is the desired angle of the joint, θ j (m) is the actual joint angle collected by the joint position detection unit.

[0065] is the differential of the actual joint angle, expressed as:

[0066]

[0067] according to Figure 2 The speed control method shown in the figure converts the compensated desired speed V cmdIn (k) is converted into the desired motor speed V m_cmd (k), and then subtract it from the actual motor speed to obtain the motor angular velocity error V m_err (k) Input speed loop proportional control, speed loop integral control and speed loop differential control respectively.

[0068] Convert the input desired speed to the motor desired speed:

[0069] V m_cmd (k) = V cmdIn (k)×N

[0070] N is the joint transmission ratio.

[0071] Motor expected angular velocity V m_cmd (k) and the actual angular velocity V of the motor m (k) Calculate the difference and get the motor angular velocity error V m_err(k):

[0072] V m_err (k) = V m_cmd (k)-V m (k)

[0073] The speed loop proportional control includes the high-frequency oscillation suppression function and the speed loop proportional link. Among them, the high-frequency oscillation suppression function can effectively suppress the oscillation characteristics during startup. The motor angular velocity error V after suppression m_errPF for:

[0074]

[0075] T Pf is the oscillation suppression coefficient, T Pf Less than 1.

[0076] Speed ​​error integral V m_errI (k) is expressed as:

[0077]

[0078] The speed loop differential term differentiates the motor angular velocity error, and the motor angular velocity differential V m_D (k) is expressed as:

[0079]

[0080] T v is the speed loop control period.

[0081] The damping link can adjust the damping characteristics of the control system, the motor speed damping V m_F (k) is described as:

[0082] V m_F (k) = K V-F ×V m (k)

[0083] 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 V-F is the damping coefficient of the speed loop controller. Its value needs to be debugged. The initial value can be set to 1 to obtain the stability margin of the flexible joint motion speed tracking system under different load conditions, and perform parameter iterative debugging according to the stability margin until a stability margin that meets the index requirements is obtained.

[0084] Integrate all control links to obtain the output expected current i of the speed loop controller Qcmd (k) is:

[0085] i Qcmd (k) = K V-P ×Vm_errPF (k)+K V-I ×V m_errI (k)+K V-D ×V m_D (k)-V m_F (k)

[0086] Among them, K V-P is the proportional coefficient of the speed loop controller, K V-I is the integral coefficient of the speed loop controller, K V-D It is the differential coefficient of the speed loop controller. The three coefficients need to be debugged synchronously. The initial values ​​can be set to 1, 0, 0 to obtain the response time, overshoot, stability margin and other parameters of the flexible joint motion speed tracking system. Based on this, the three coefficients are iteratively debugged until the parameters that meet the index requirements are obtained.

[0087] Current loop controller: receives the output desired current i transmitted by the speed loop controller Qcmd (k), receiving the actual current i corresponding to the motor output voltage fed back by the motor drive module Q (k), according to the expected output current i Qcmd (k) and the actual current i Q (k) Adjust the drive voltage output to the motor drive module so that the actual current i Q (k) Continuously output the expected current i Qcmd (k) Follow-up. Figure 1 Middle U D is the direct axis voltage, U Q is the quadrature axis voltage.

[0088] The present invention uses the output desired current of the speed loop controller and can adopt different motor driving methods to drive the motor, thereby achieving the motion control effect of the joint to achieve the effect of tracking the desired position or desired speed of the joint.

[0089] 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 of the motor m (k) to the speed loop controller; feedback the actual current i corresponding to the motor output voltage Q (k) To the current loop controller.

[0090] Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make possible changes and modifications to the technical solution of the present invention by using the above disclosed methods and technical contents without departing from the spirit and scope of the present invention. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solution of the present invention shall belong to the protection scope of the technical solution of the present invention. In the absence of conflict, the embodiments of the present application and the technical features in the embodiments can be combined with each other.

[0091] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A flexible joint motion speed tracking system for large inertia variable load in space, characterized in that: include: Joint position detection unit, position loop controller, speed loop controller, current loop controller and motor drive module; Position loop controller: According to the desired joint angle and the actual joint angle collected by the joint position detection unit, the position loop control cycle T P Determine the compensation speed V conp (m) and transmitted to the speed loop controller; Speed ​​loop controller: receives the compensation speed V transmitted by the position loop controller conp (m), the expected joint velocity V receiving the input cmd (k), according to the compensation speed V conp (m) The desired joint velocity V according to the velocity loop control period Tv cmd (k) Compensate and obtain the expected speed V after compensation cmdIn (k); According to the expected speed V after compensation cmdIn (k) and the actual angular velocity V of the motor fed back by the motor drive module m (k), determine the expected output current i Qcmd (k) and transmitted to the current loop controller; Current loop controller: receives the output desired current i transmitted by the speed loop controller Qcmd (k), receiving the actual current i corresponding to the motor output voltage fed back by the motor drive module Q (k), according to the expected output current i Qcmd (k) and the actual current i Q (k) Adjust the drive voltage output to the motor drive module so that the actual current i Q (k) Continuously output the expected current i Qcmd (k) conduct tracking; Motor drive module: receives the drive voltage output by the current loop controller, drives the motor to drive the joint to rotate according to the drive voltage; feeds back the actual angular velocity V of the motor m (k) to the speed loop controller; feedback the actual current i corresponding to the motor output voltage Q (k) to the current loop controller; It is characterized in that the speed loop controller is based on the compensated desired speed V cmdIn (k) and the actual angular velocity V of the motor m (k), determine the expected output current i Qcmd (k) specifically: The speed loop controller obtains the desired speed V after compensation. cmdIn (k) and the actual angular velocity of the motor Vm(k), and obtain 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 the motor speed damping V m_F (k); 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), determine the expected output current i Q cmd(k); It is characterized in that the speed loop controller obtains the suppressed motor angular velocity error V m_errPF The details are 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 oscillation suppression coefficient, T Pf Less than 1, N is the joint transmission ratio; The speed loop controller obtains the speed error integral value V m_errI (k) are as follows:

2. The flexible joint motion speed tracking system for large inertia variable load in space according to claim 1 is characterized in that: The position loop controller determines the compensation speed V conp (m), specifically: According to the joint expected angle θ of the current mth position loop control cycle cmd (m) and the actual joint angle θ j (m), determine the angle error θ of the current mth position loop control cycle err (m); According to the actual joint angle θ of the current mth position loop control cycle j (m) and the actual joint angle θ of the m-1th position loop control cycle j (m-1), determine the differential value of the actual joint angle of the current mth position loop control cycle According to the angle error θ err (m) and the differential value of the actual angle of the joint Determine the compensation speed V conp (m).

3. The flexible joint motion speed tracking system for large inertia variable load in space according to claim 2 is characterized in that: The position loop controller determines the angle error θ of the current mth position loop control cycle err (m), specifically: i err (m)=θ cmd (m)-θ j (m)。 4. The flexible joint motion speed tracking system for large inertia variable load in space according to claim 2 is 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. The flexible joint motion speed tracking system for large inertia variable load in space according to claim 2 is characterized in that: The position loop controller determines the compensation speed V conp (m), specifically: Among them, K P-P is the proportional coefficient of the position loop controller, K P-D is the differential coefficient of the position loop controller.

6. The flexible joint motion speed tracking system for large inertia variable load in space according to claim 1 is characterized in that: The speed loop controller is responsible for the joint's desired speed V cmd (k) Compensate and obtain the expected speed V after compensation cmdIn (k) specifically: V cmdIn (k)=V cmd (k)+V conp (m) Among them, in the position loop control period T P and speed loop control period T v When the ratio Q is 1, m = k; When Q is a positive integer greater than 1: Wherein, n is a positive integer.

7. The flexible joint motion speed tracking system for large inertia variable load in space according to claim 1 is characterized in that: The speed loop controller obtains the motor angular velocity differential value V m_D (k) are as follows: V m_err (k)=V m_cmd (k)-V m (k) V m_cmd (k)=V cmdIn (k)×N Wherein, N is the joint transmission ratio; The speed loop controller obtains the motor speed damping V m_F (k) are as follows: In m_F (k)=K V-F ×V m (to) Among them, K V-F is the damping coefficient of the speed loop controller.

8. A flexible joint motion speed tracking system for large inertia variable load in space according to any one of claims 1 and 7, characterized in that: The speed loop controller determines the output desired current i Qcmd (k) specifically: and Qcmd (k)=K V-P ×V m_errPF (k)+K V-I ×V m_errI (k)+K V-D ×V m_D (k)-V m_F (k) among them, K V-P is the proportional coefficient of the speed loop controller, K V-I is the integral coefficient of the speed loop controller, K V-D is the differential coefficient of the speed loop controller.

Images