An Inertial Navigation Modulation Axis Control Method Based on LQR Regulator

Abstract

The present invention discloses an inertial navigation modulation axis control method based on an LQR regulator, which relates to the technical field of inertial navigation, and includes: sampling the current angular velocity and the angular position of the motor and respectively taking the differences from the expected values of the inertial navigation system, and taking the two differences as the angular velocity error and the angular position error respectively and inputting them into the LQR regulator; taking the state equation of the motor as the objective function in the LQR regulator, generating and solving the Riccati equation, inputting the expected acceleration of the motor into the PID controller and tracking the expected acceleration, taking the difference between the expected acceleration and the currently sampled acceleration of the motor, and calculating the control quantity as the output of the PID controller, and the control quantity is the Q-axis control voltage of the motor in the rotating coordinate system; the inertial navigation system calculates and processes the control quantity through the SVPWM algorithm to control the modulation axis of the inertial navigation device, and the modulation axis issues an instruction to the motor to correct the operating state of the motor.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation, and more particularly, to an inertial navigation modulation axis control method based on an LQR regulator. Background Art

[0002] Inertial navigation is a completely autonomous navigation technology that relies on inertial devices installed on a carrier to autonomously complete navigation tasks through a navigation computer, with good concealment and no environmental restrictions on the working environment.

[0003] An inertial navigation system uses an inertial measurement unit mainly composed of gyroscopes and accelerometers to measure the motion of a carrier relative to inertial space; the rotation modulation technology belongs to the category of system error compensation, and uses a rotation mechanism to rotate periodically, so that the errors of inertial sensitive elements are modulated into a periodic oscillation form, and they cancel each other out within a rotation period, so as to achieve the purpose of error suppression and significantly improve the use accuracy.

[0004] In the field of celestial navigation, permanent magnet synchronous motors are mostly used for the inertial navigation modulation axis system. During navigation, it is necessary to drive the inertial navigation device to rotate periodically between certain angles. Currently, a three-loop control structure of D / Q axis current loop PID, speed loop PID, position loop PID or path planning is mostly used. This method involves many parameters, requires a large amount of debugging work, depends on experience, and the parameters also need to be adjusted accordingly after the environment changes, further increasing the workload. Summary of the Invention

[0005] The purpose of the present invention is to propose a control method based on an LQR controller, obtain an optimal control rate for the objective function, and reduce the number of parameters to be debugged.

[0006] The technical solution of the present invention is to provide an inertial navigation modulation axis control method based on an LQR regulator, and the method includes:

[0007] S1. Sample the current angular velocity and the angular position of the motor shaft, subtract the expected angular velocity and the expected angular position output by the inertial navigation system from the sampled angular velocity and angular position respectively, and input the two differences as the angular velocity error and the angular position error into the LQR regulator respectively;

[0008] S2. Use the state equation of the motor as the objective function in the LQR regulator, generate and solve the Riccati equation, and use the initial expected acceleration of the motor as the output of the LQR regulator;

[0009] S3. Limit the initial modulation acceleration output by the LQR regulator to obtain the expected acceleration, and input the expected acceleration into the PID controller;

[0010] S4. The PID controller tracks the desired acceleration, calculates the difference between the desired acceleration and the currently sampled motor acceleration, and computes the control variable u q (t) as the output of the PID controller. The control variable u q (t) is the Q-axis control voltage of the motor in the rotating coordinate system;

[0011] S5. The inertial navigation system processes the control variable u q (t) through the SVPWM algorithm to control the modulation axis of the inertial navigation device, and the modulation axis issues commands to the motor to correct the operating state of the motor.

[0012] In any of the above technical solutions, further, the state equation of the motor is represented in vector form as:

[0013]

[0014] where e θ is the angle error, e ω is the angular velocity error, u(t) is the input acceleration, and t is the time.

[0015] In any of the above technical solutions, further, the Riccati equation is:

[0016]

[0017] where represents the non-negative definite steady-state solution of the Riccati equation, A represents the state transition matrix, B represents the input matrix, Q is the error cost term matrix of the LQR regulator, and R is the input cost term matrix of the LQR regulator;

[0018] Thus, the initial desired acceleration u * (t) is obtained:

[0019]

[0020] where x(t) is the error vector.

[0021] In any of the above technical solutions, further, the clipping rule for clipping in step S3 is as follows:

[0022]

[0023] where u max is the maximum acceleration preset by the inertial navigation system.

[0024] In any of the above technical solutions, further, the calculation process of the control variable u q (t) is as follows:

[0025]

[0026] where \(e(t)\) is the desired acceleration \(u\) a (t) is the difference between the current acceleration of the motor obtained by sampling, \(t\) is the current time, \(\tau\) is the intermediate time, \(k_p\) is the proportional term coefficient of the PID, \(k_i\) is the integral term coefficient of the PID, and \(k_d\) is the derivative term coefficient of the PID.

[0027] The beneficial effects of the present invention are:

[0028] In the technical solution of the present invention, by using an LQR regulator with angular / velocity error as the input and modulated acceleration as the output, the use of redundant PID controllers in the prior art is greatly reduced, the workload required for debugging parameters is greatly reduced, and the reliability of the inertial navigation system is also improved; an optimal control law is obtained for the selected objective function to make up for the accuracy loss. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The above and additional aspects of the present invention will become obvious and easy to understand in the description of the embodiments in conjunction with the following drawings, where:

[0030] Figure 1 is a schematic flow chart of an inertial navigation modulation axis control method based on an LQR regulator according to an embodiment of the present invention;

[0031] Figure 2 is a system control block diagram of an inertial navigation modulation axis control method based on an LQR regulator according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0032] In order to be able to more clearly understand the above objects, features and advantages of the present invention, the present invention will be further described in detail below in conjunction with the drawings and specific embodiments. It should be noted that, without conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other.

[0033] In the following description, many specific details are set forth in order to fully understand the present invention. However, the present invention can also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited by the specific embodiments disclosed below.

[0034] The linear quadratic regulator, abbreviated as the LQR regulator, is for a linear system given in state space form in modern control theory, and the objective function is a quadratic function of the object state and the control input. The LQR regulator can obtain the optimal control law of state linear feedback and is easy to form a closed-loop optimal control.

[0035] As Figure 1 shown, this embodiment provides an inertial navigation modulation axis control method based on an LQR regulator, and the method includes:

[0036] S1. Sample the current angular velocity and the angular position of the motor. The inertial navigation system outputs the desired angular velocity and the desired angular position, and respectively calculates the differences from the sampled angular velocity and angular position. These two differences are used as the angular velocity error and the angular position error and input into the LQR regulator respectively.

[0037] Specifically, sample the current angular velocity and the angular position of the motor shaft system, calculate the difference from the actual value and the expected value, which can be used as the input of the LQR regulator. The reference point or line of the angular position is related to the setting during the installation of the sensor.

[0038] S2. Use the state equation of the motor as the objective function in the LQR regulator, generate and solve the Riccati equation, and output the initial desired acceleration of the motor.

[0039] Specifically, the state equation of the motor is represented in vector form as:

[0040]

[0041] where e θ is the angular position error, e ω is the angular velocity error, u(t) is the input acceleration, and t is the time.

[0042] After selecting the error cost term matrix Q and the input cost term matrix R of the LQR regulator, there is the following performance index:

[0043]

[0044] where J is the cost function and x(t) is the error.

[0045] The generated Riccati equation is:

[0046]

[0047] where represents the non - negative definite steady - state solution of the Riccati equation, A represents the state transition matrix, and B represents the input matrix.

[0048] Obtain the non - negative definite steady - state solution P by solving the Riccati equation, and obtain the initial desired acceleration u * (t):

[0049]

[0050] Take this acceleration u * (t) as the output of the LQR regulator.

[0051] S3. Take the initial desired acceleration u output by the LQR regulator* The clipping is performed on (t) to obtain the desired acceleration u a (t), and the desired acceleration u a (t) is input into the PID controller.

[0052] Specifically, the clipping rule is as follows:

[0053]

[0054] where u max is the maximum acceleration preset by the inertial navigation system.

[0055] S4. The PID controller tracks the desired acceleration u a (t), calculates the difference between the desired acceleration u a (t) and the currently sampled motor acceleration, and calculates the control quantity u q (t) as the output of the PID controller. The control quantity u q (t) is the Q-axis control voltage of the motor in the rotating coordinate system.

[0056] The calculation process of the control quantity u q (t) is as follows:

[0057]

[0058] where e(t) is the difference between the desired acceleration u a (t) and the currently sampled motor acceleration, t is the current time, τ is the intermediate time, kp is the proportional term coefficient of the PID, ki is the integral term coefficient of the PID, and kd is the derivative term coefficient of the PID.

[0059] S5. The inertial navigation system calculates and processes the control quantity u q (t) through the SVPWM algorithm, controls the modulation axis of the inertial navigation device, and the modulation axis issues an instruction to the motor to correct the operating state of the motor.

[0060] The SVPWM algorithm is the abbreviation of the Space Vector Pulse Width Modulation algorithm. It is a pulse width modulation wave generated by a specific switching mode composed of six power switching elements of a three-phase power inverter, which can make the output current waveform as close as possible to the ideal sine waveform. The harmonic components of the winding current waveform processed by the SVPWM algorithm are small, which reduces the torque ripple of the motor, makes the rotating magnetic field closer to a circle, and greatly improves the utilization rate of the DC bus voltage and is more easily realized digitally.

[0061] Compared with the commonly used triple-loop control structure, it is necessary to debug a variety of parameters including 4 sets of PID, which has a large workload and requires readjustment after environmental changes. The method provided by the present invention only needs to debug the following parameters: the Q and R matrices of the LQR regulator, and the kp, ki, and kd parameters of the PID controller; the number of parameters is small, the debugging workload is small, and the optimal control is achieved for the selected objective function.

[0062] Among them, the diagonal coefficients of the Q matrix are used to adjust the cost terms of the position and speed phase errors, and the R matrix is used to adjust the cost term of the control quantity. The Q and R matrices of the LQR regulator can be theoretically obtained through simulation, and the kp, ki, and kd parameters of the PID controller can be obtained through physical debugging.

[0063] In summary, the present invention proposes an inertial navigation modulation axis control method based on an LQR regulator, and the method includes:

[0064] S1. Sample the current angular velocity and the angular position of the motor, the inertial navigation system outputs the expected angular velocity and the expected angular position and calculates the differences with the sampled angular velocity and angular position respectively, and input the two differences as the angular velocity error and the angular position error into the LQR regulator respectively.

[0065] S2. Take the state equation of the motor as the objective function in the LQR regulator, generate and solve the Riccati equation, and take the initial expected acceleration of the motor as the output of the LQR regulator.

[0066] S3. Limit the initial expected acceleration output by the LQR regulator to obtain the expected acceleration, and input the expected acceleration into the PID controller.

[0067] S4. The PID controller tracks the expected acceleration, calculates the difference between the expected acceleration and the currently sampled acceleration of the motor, and calculates the control quantity as the output of the PID controller. The control quantity is the Q-axis control voltage of the motor in the rotating coordinate system.

[0068] S5. The inertial navigation system calculates and processes the control quantity through the SVPWM algorithm, controls the modulation axis of the inertial navigation device, and the modulation axis issues an instruction to the motor to correct the operating state of the motor.

[0069] In the present invention, terms such as "installation", "connection", "connection", and "fixation" should be understood in a broad sense. For example, "connection" can be a fixed connection, a detachable connection, or an integral connection; "connection" can be a direct connection or an indirect connection through an intermediate medium. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0070] The steps in the present invention can be adjusted, combined, and deleted according to actual needs.

[0071] Although the present application is disclosed in detail with reference to the accompanying drawings, it should be understood that these descriptions are merely exemplary and are not intended to limit the application of the present application. The scope of protection of the present application is defined by the appended claims and may include various variations, modifications, and equivalent solutions to the invention without departing from the scope and spirit of the present application.

Claims

1. An inertial navigation modulation axis control method based on an LQR regulator, characterized in that, The method includes: S1. Sample the current angular velocity and the current angular position of the motor. The inertial navigation system outputs the desired angular velocity and the desired angular position, and calculates the differences from the sampled angular velocity and angular position respectively. Then, use the two differences as the angular velocity error and the angular position error and input them into the LQR regulator respectively; S2. Use the state equation of the motor as the objective function in the LQR regulator, generate and solve the Riccati equation, and use the initial desired acceleration of the motor as the output of the LQR regulator; S3. Limit the initial desired acceleration output by the LQR regulator to obtain the desired acceleration, and input the desired acceleration into the PID controller; S4. The PID controller tracks the desired acceleration, calculates the difference between the desired acceleration and the currently sampled motor acceleration, and calculates the control variable u q (t) as the output of the PID controller, where the control variable u q (t) is the Q-axis control voltage of the motor in the rotating coordinate system; S5. The inertial navigation system calculates and processes the control quantity u q (t) through the SVPWM algorithm to control the modulation axis of the inertial navigation device, and the modulation axis issues commands to the motor to correct the operating state of the motor.

2. The inertial navigation modulation axis control method based on the LQR regulator according to claim 1, characterized in that The state equation of the motor is represented in vector form as: where e θ is the angular error, e ω is the angular velocity error, u(t) is the input acceleration, and t is the time.

3. The inertial navigation modulation axis control method based on the LQR regulator according to claim 1, characterized in that The Riccati equation is: where represents the non - negative steady - state solution of the Riccati equation, A represents the state transition matrix, B represents the input matrix, Q is the error cost term matrix of the LQR regulator, and R is the input cost term matrix of the LQR regulator; Thus, the initial desired acceleration u * (t): where x(t) is the error vector.

4. The inertial navigation modulation axis control method based on the LQR regulator according to claim 3, wherein, The limiting rule for the limiting in step S3 is as follows: where u max is the maximum acceleration preset by the inertial navigation system.

5. The inertial navigation modulation axis control method based on the LQR regulator according to claim 1, characterized in that, The control quantity u q (t) is calculated as follows: where e(t) is the difference between the desired acceleration u a (t) and the currently sampled motor acceleration, t is the current time, τ is the intermediate time, kp is the proportional term coefficient of the PID, ki is the integral term coefficient of the PID, and kd is the derivative term coefficient of the PID.

Images