A Servo System Control Method Based on Kalman Filter

Abstract

The present invention discloses a servo system control method based on Kalman filtering, comprising the following steps: 1. obtaining a command signal at the current moment; 2. obtaining a feedback signal at the current moment; 3. obtaining two output signals of the Kalman filter at the previous moment; 4. filtering the command signal using the Kalman filter to obtain a filtered command signal and a differential of the command signal; 5. calculating a new command signal using the filtered command signal and the differential of the command signal; 6. calculating a tracking error using the new command signal and the feedback signal; and 7. calculating a controller output using the tracking error. The present invention overcomes the inherent delay of the feedback controller itself and has better command tracking performance.

Description

Technical Field

[0001] The present invention belongs to the field of control technology, and in particular relates to a servo system control method based on Kalman filtering. Background Art

[0002] Servo system control methods are central to achieving high-performance tracking of commands. Existing servo system control methods generally enhance tracking performance by improving the feedback controller. However, due to the inherent delay characteristics of the feedback controller, existing methods inevitably suffer from a trade-off between tracking speed and overshoot. Summary of the Invention

[0003] To overcome the shortcomings of the existing technology, the present invention provides a servo system control method based on Kalman filtering, comprising the following steps: 1. obtaining a command signal at the current moment; 2. obtaining a feedback signal at the current moment; 3. obtaining two output signals of the Kalman filter at the previous moment; 4. filtering the command signal using the Kalman filter to obtain a filtered command signal and its differential; 5. calculating a new command signal using the filtered command signal and its differential; 6. calculating a tracking error using the new command signal and the feedback signal; and 7. calculating a controller output using the tracking error. The present invention overcomes the inherent delay of the feedback controller and achieves improved command tracking performance.

[0004] The technical solutions adopted by the present invention to solve the technical problems are as follows:

[0005] Step 1: Get the current time t = kT s The command signal s(k) at time T s is the duration of the control cycle, k is the sequence number of the moment, the value range is k=1,2,3…, s(k) is t=kT s command signals at all times;

[0006] Step 2: Get the current time t = kT s The feedback signal f(k) at time ;

[0007] Step 3: Obtain the two output signals of the Kalman filter at the previous moment, which are the filtered command signals x1(k-1) and v1(k-1);

[0008] When k=1, there is no signal at the previous moment, so let x1(k-1)=0, v1(k-1)=0;

[0009] Get the update matrix P(k-1) of the previous moment. When k=1, there is no update matrix of the previous moment. At this time, let:

[0010]

[0011] Step 4: Use the Kalman filter to filter the acquired command signal s(k), and output the filtered command signal x1(k) and the differential of the command signal v1(k);

[0012] Step 5: Calculate the new command signal s using the filtered command signal x1(k) and the differential of the command signal v1(k) ′ (k);

[0013] Step 6: Utilize s ′ (k) and the feedback signal f(k) to calculate the tracking error e(k);

[0014] Step 7: Using e(k) as input, use the controller to calculate the control variable u(k).

[0015] Furthermore, the Kalman filter comprises the following steps:

[0016] Step 4-1: Calculate the auxiliary matrix Q according to the following formula:

[0017] Q=FP(k-1)F T

[0018] in, F T is the transpose of matrix F;

[0019] Step 4-2: Calculate the auxiliary variable y according to the following formula:

[0020]

[0021] Where H = [1 0];

[0022] Step 4-3: Calculate the auxiliary matrix K according to the following formula:

[0023] K=QH T (HQH T +1) -1

[0024] Among them, (HQH T +1) -1 for (HQH T +1);

[0025] Step 4-4: Calculate the Kalman filter output x1(k) and v1(k) according to the following formula:

[0026]

[0027] Furthermore, the new command signal s ′ k (k) is calculated according to the following formula:

[0028] s ′ (k)=x1(k)+k s v1(k).

[0029] Furthermore, the tracking error e(k) is calculated as follows:

[0030] e(k)=s ′ (k)-f(k).

[0031] The beneficial effects of the present invention are as follows:

[0032] (1) Different from the existing methods for improving the feedback controller, the method provided by the present invention constructs a new command signal based on Kalman filtering, overcomes the inherent delay of the feedback controller itself, and has better command tracking performance.

[0033] (2) Using Kalman filtering to estimate the differential of the command signal has better smoothness. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 This is a block diagram of a servo system control method based on Kalman filtering according to the present invention;

[0035] Figure 2 This is a flow chart of the servo system control method based on Kalman filtering of the present invention. DETAILED DESCRIPTION

[0036] The present invention will be further described below with reference to the accompanying drawings and examples.

[0037] The present invention provides a servo system control method based on Kalman filtering, comprising the following steps:

[0038] S1. Get the current time t=kT s The command signal s(k) at time T s is the duration of the control cycle, k is the sequence number of the moment, the value range is k=1,2,3…, s(k) is t=kT s command signals at all times;

[0039] S2. Get the current time t=kT s The feedback signal f(k) at ;

[0040] S3. Obtain the two output signals of the Kalman filter at the previous moment, which are the filtered command signals x1(k-1) and v1(k-1). When k=1, there is no signal at the previous moment. In this case, let x1(k-1)=0 and v1(k-1)=0. Obtain the update matrix P(k-1) at the previous moment. When k=1, there is no update matrix at the previous moment. In this case, let:

[0041]

[0042] S4. Use the Kalman filter to filter the acquired command signal s(k), and output the filtered command signal x1(k) and the differential of the command signal v1(k);

[0043] S5. Calculate the new command signal s using the filtered command signal x1(k) and the differential of the command signal v1(k) ′ (k);

[0044] S6, use s ′ (k) and the feedback signal f(k) to calculate the tracking error e(k);

[0045] S7. Using e(k) as input, use the controller to calculate the control variable u(k).

[0046] Furthermore, the Kalman filter in step S4 has the following steps:

[0047] First, calculate the auxiliary matrix Q according to the following formula:

[0048] Q=FP(k-1)F T

[0049] in, F T is the transpose of the matrix F.

[0050] Then calculate the auxiliary variable y according to the following formula:

[0051]

[0052] Where H = [1 0];

[0053] Then calculate the auxiliary matrix K according to the following formula:

[0054] K=QH T (HQH T +1) -1

[0055] Among them, (HQH T +1) -1 For (HQH T +1).

[0056] Then calculate the output x1(k) and v1(k) of the Kalman filter as follows:

[0057]

[0058] Furthermore, in step S5, the new command signal s ′k (k) is calculated according to the following formula:

[0059] s ′ (k)=x1(k)+k s v1(k);

[0060] Furthermore, the tracking error e(k) in step S6 is calculated according to the following formula:

[0061] e(k)=s ′ (k)-f(k);

[0062] Example:

[0063] The block diagram of the servo system control method based on Kalman filtering provided by the present invention is as follows: Figure 1 As shown in the figure, it can be seen that the servo system method based on the Kalman filter provided by the present invention uses the Kalman filter to process the original command signal. The Kalman filter realizes two functions: one is to smooth the command signal, reduce the impact of the high-frequency components in the command signal on the system, and reduce the oscillation that may be generated during the command tracking process; the other is to estimate the differential of the command signal. The two outputs of the Kalman filter are then algebraically combined, that is, the differential signal is multiplied by a gain and then added to the filtered command signal to form a new command signal. The new command signal is subtracted from the collected feedback signal to obtain a tracking error signal. The controller uses the error signal to calculate the control quantity, and finally drives the servo system to track the command signal.

[0064] The present invention provides a servo system control method based on Kalman filtering. Figure 2 The specific steps are as follows:

[0065] First, get the current time t = kT s The command signal s(k) at time t is the current time, T s is the duration of the control cycle, k is the sequence number of the moment, the value range is k=1,2,3…, s(k) is t=kT s command signals at all times;

[0066] Further, obtain the current time t = kT s The feedback signal f(k) at time ;

[0067] Furthermore, the two output signals of the Kalman filter at the previous moment are obtained, which are the filtered command signals x1(k-1) and v1(k-1). When k=1, there is no signal at the previous moment, so let x1(k-1)=0, v1(k-1)=0. Get the update matrix P(k-1) at the previous moment. When k=1, there is no update matrix at the previous moment, so let

[0068]

[0069] Furthermore, the Kalman filter is calculated as follows:

[0070] First, calculate the auxiliary matrix Q according to the following formula:

[0071] Q=FP(k-1)F T

[0072] in, F T is the transpose of matrix F;

[0073] Then calculate the auxiliary variable y according to the following formula:

[0074]

[0075] Where H = [1 0];

[0076] Then calculate the auxiliary matrix K according to the following formula:

[0077] K=QH T (HQH T +1) -1

[0078] Among them, (HQH T +1) -1 For (HQH T +1).

[0079] Then calculate the output x1(k) and v1(k) of the Kalman filter as follows:

[0080]

[0081] Furthermore, the acquired command signal is filtered using a Kalman filter, and the filtered command signal x1(k) and the differential of the command signal v1(k) are output;

[0082] Furthermore, the new command signal s is calculated using the filtered command signal x1(k) and the differential of the command signal v1(k) according to the following formula: ′ (k):

[0083] s ′ (k)=x1(k)+k s v1(k)

[0084] Further, using s ′ (k) and the feedback signal f(k) are used to calculate the tracking error e(k) as follows:

[0085] e(k)=s ′(k)-f(k)

[0086] Furthermore, with e(k) as input, a feedback controller is used to calculate the control variable u(k). The feedback controller is not limited to its specific form, as long as its input signal is the tracking error e(k), such as a proportional controller, a proportional-integral-differential controller, etc. Taking the proportional controller as an example, the output of the controller is:

[0087] u(k)=k p e(k)

[0088] where k p is the proportional gain.

Claims

1. A servo system control method based on Kalman filtering, characterized in that: The steps include: Step 1: Get the current time t = kT s The command signal s(k) at time T s is the duration of the control cycle, k is the sequence number of the moment, the value range is k=1,2,3…, s(k) is t=kT s command signals at all times; Step 2: Get the current time t = kT s The feedback signal f(k) at ; Step 3: Obtain the two output signals of the Kalman filter at the previous moment, which are the filtered command signals x1(k-1) and v1(k-1); When k=1, there is no signal at the previous moment, so let x1(k-1)=0, v1(k-1)=0; Get the update matrix P(k-1) of the previous moment. When k=1, there is no update matrix of the previous moment. At this time, let: Step 4: Use the Kalman filter to filter the acquired command signal s(k), and output the filtered command signal x1(k) and the differential of the command signal v1(k); Step 4-1: Calculate the auxiliary matrix Q according to the following formula: Q=F P(k-1)F T in, F T is the transpose of matrix F; Step 4-2: Calculate the auxiliary variable y according to the following formula: Where H = [1 0]; Step 4-3: Calculate the auxiliary matrix K according to the following formula: K=QH T (HQH T +1) -1 Among them, (HQH T +1) -1 for (HQH T +1); Step 4-4: Calculate the Kalman filter output x1(k) and v1(k) according to the following formula: Step 5: Calculate the new command signal s using the filtered command signal x1(k) and the differential of the command signal v1(k) ′ (k); Step 6: Utilize s ′ (k) and the feedback signal f(k) to calculate the tracking error e(k); Step 7: Using e(k) as input, use the controller to calculate the control variable u(k).

2. A servo system control method based on Kalman filtering according to claim 1, characterized in that: New command signal s ′ k (k) is calculated according to the following formula: s ′ (k)=x1(k)+k s v1(k).

3. The servo system control method based on Kalman filtering according to claim 2, characterized in that: The tracking error e(k) is calculated as follows: e(k)=s ′ (k)-f(k)。

Images