CN115903507A - Inertial navigation modulation axis control method based on LQR regulator - Google Patents
Inertial navigation modulation axis control method based on LQR regulator Download PDFInfo
- Publication number
- CN115903507A CN115903507A CN202211537766.4A CN202211537766A CN115903507A CN 115903507 A CN115903507 A CN 115903507A CN 202211537766 A CN202211537766 A CN 202211537766A CN 115903507 A CN115903507 A CN 115903507A
- Authority
- CN
- China
- Prior art keywords
- lqr
- regulator
- motor
- inertial navigation
- acceleration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Control Of Electric Motors In General (AREA)
Abstract
The invention discloses an inertial navigation modulation axis control method based on an LQR regulator, which relates to the technical field of inertial navigation and comprises the following steps: sampling the current angular velocity and the current angular position of the motor, respectively calculating the difference with the expected value of the inertial navigation system, and respectively inputting the two difference values serving as an angular velocity error and an angular error into an LQR regulator; taking a state equation of a motor as a target function in an LQR regulator, generating a Riccati equation and solving the Riccati equation, inputting the expected acceleration of the motor into a PID controller, tracking the expected acceleration, calculating the difference between the expected acceleration and the current acceleration of the motor obtained by sampling, and calculating a control quantity as the output of the PID controller, wherein the control quantity is the Q-axis control voltage of the motor under a rotating coordinate system; the inertial navigation system calculates the processing control quantity through an SVPWM algorithm, controls a modulation shaft of the inertial navigation equipment, and the modulation shaft sends an instruction to the motor so as to correct the running state of the motor.
Description
Technical Field
The invention relates to the technical field of inertial navigation, in particular to an inertial navigation modulation axis control method based on an LQR (Linear quadratic response) regulator.
Background
The inertial navigation is a completely autonomous navigation technology, and the inertial device mounted on the carrier autonomously completes the navigation task through a navigation computer, so that the invisibility is good, and the working environment is not limited by the environment.
The inertial navigation system measures the motion of a carrier relative to an inertial space by using an inertial measurement unit mainly comprising a gyroscope and an accelerometer; the rotation modulation technology belongs to the field of system error compensation, and utilizes the periodic rotation of a transposition mechanism to modulate the error of an inertia sensitive element into a periodic oscillation form so as to mutually counteract the error in a rotation period, thereby achieving the purpose of error inhibition and obviously improving the use precision.
In the field of astronomical navigation, a permanent magnet synchronous motor is mostly adopted by an inertial navigation modulation shaft system. During navigation, the inertial navigation equipment needs to be driven to periodically rotate circularly between certain angles. At present, three loop control structures of a D/Q axis current loop PID, a speed loop PID, a position loop PID or path planning are mostly adopted. The method has more parameters, large debugging workload and dependence on experience, and the parameters also need to be correspondingly adjusted after the environment changes, thereby further increasing the workload.
Disclosure of Invention
The invention aims to: a control method based on an LQR controller is provided, an optimal control rate is obtained aiming at a target function, and the number of parameters needing debugging is reduced.
The technical scheme of the invention is as follows: the inertial navigation modulation axis control method based on the LQR regulator comprises the following steps:
s1, sampling the current angular speed and the current angular position of a motor zhou, outputting the expected angular speed and the expected angular position by an inertial navigation system, respectively calculating the difference between the expected angular speed and the expected angular position and the sampled angular speed and the sampled angular position, and respectively inputting the two difference values serving as an angular speed error and an angular error into an LQR regulator;
s2, taking a state equation of the motor as a target function in the LQR regulator, generating a Riccati equation, solving the Riccati equation, and taking the initial expected acceleration of the motor as the output of the LQR regulator;
s3, carrying out amplitude limiting on the initial modulation acceleration output by the LQR regulator to obtain an expected acceleration, and inputting the expected acceleration into a PID controller;
s4, the PID controller tracks the expected acceleration, the difference between the expected acceleration and the sampled current acceleration of the motor is obtained, and the control quantity u is calculated q (t) as output of the PID controller, the control quantity u q (t) controlling voltage of a Q axis of the motor under a rotating coordinate system;
s5, calculating and processing the control quantity u by the inertial navigation system through an SVPWM algorithm q And (t) controlling a modulation shaft of the inertial navigation equipment, and enabling the modulation shaft to send a command to the motor so as to correct the running state of the motor.
In any one of the above technical solutions, further, the state equation of the motor is expressed by a vector equation as:
wherein e θ As an angle error, e ω For angular velocity error, u (t) is input magnitude acceleration, and t is time.
In any of the above technical solutions, further, the Riccati equation is:
wherein
Expressing an non-negative constant solution of a Riccati equation, wherein A represents a state transition matrix, B represents an input matrix, Q is an error cost item matrix of the LQR regulator, and R is an input cost item matrix of the LQR regulator;
thereby obtaining an initial desired acceleration u * (t)：
Where x (t) is the error vector.
In any of the above technical solutions, further, the clipping rule of the clipping in step S3 is as follows:
wherein u is max Is the maximum acceleration preset by the inertial navigation system.
In any of the above technical solutions, further, the control quantity u q The calculation procedure of (t) is as follows:
where e (t) is the desired acceleration u a (t) the difference value of the current acceleration of the motor obtained by sampling, wherein t is the current moment, tau is the intermediate moment, kp is the proportional term coefficient of PID, ki is the integral term coefficient of PID, and kd is the differential term coefficient of PID.
The invention has the beneficial effects that:
in the technical scheme of the invention, the LQR regulator which takes the angle/speed error as input and takes the modulation acceleration as output greatly reduces the use of redundant PID controllers in the prior art, greatly reduces the workload required by parameter debugging and also improves the reliability of the inertial navigation system; and aiming at the selected target function, an optimal control rate is obtained to make up for the precision loss.
Drawings
The above and additional aspects and advantages of the present invention will become apparent and readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings of which:
FIG. 1 is a schematic flow diagram of an LQR regulator-based inertial navigation modulation axis control method according to an embodiment of the invention;
fig. 2 is a system control block diagram of an LQR regulator-based inertial navigation modulation axis control method according to an embodiment of the present invention.
Detailed Description
In order that the above objects, features and advantages of the present invention can be more clearly understood, a more particular description of the invention will be rendered by reference to the appended drawings. It should be noted that the embodiments of the present invention and features of the embodiments may be combined with each other without conflict.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, however, the present invention may be practiced otherwise than as specifically described herein and, therefore, the scope of the present invention is not limited by the specific embodiments disclosed below.
The linear quadratic regulator, LQR regulator for short, is a linear system given in the state space form in modern control theory, and the target function is the quadratic function of the object state and the control input, so that the LQR regulator can obtain the optimal control rule of state linear feedback and is easy to form closed loop optimal control.
As shown in fig. 1, the present embodiment provides an inertial navigation modulation axis control method based on an LQR regulator, where the method includes:
s1, sampling the current angular speed and the current angular position of a motor, outputting the expected angular speed and the expected angular position by an inertial navigation system, respectively calculating the difference between the expected angular speed and the angle position and the sampled angular speed and angle position, and respectively inputting the two difference values serving as an angular speed error and an angular error into an LQR regulator.
Specifically, the current angular velocity and the angular position of the shafting of the motor are sampled and used as the actual value and the expected value to be subtracted, and the difference can be used as the input quantity of the LQR regulator, wherein the reference point or line of the angular position is related to the setting of the sensor during installation.
And S2, generating and solving a Riccati equation by taking a state equation of the motor as a target function in the LQR regulator, and outputting the initial expected acceleration of the motor.
Specifically, the state equation of the motor is expressed by a vector equation as:
wherein e θ As an angle error, e ω The angular velocity error is u (t), the input acceleration is u (t), and t is time.
After an error cost item matrix Q and an input cost item matrix R of the LQR regulator are selected, the following performance indexes are provided:
where J is the cost function and x (t) is the error.
The generated Riccati equation is:
wherein
Representing a non-negative constant solution of the Riccati equation, a represents the state transition matrix, and B represents the input matrix.
Obtaining an nonnegative definite constant solution P by solving the Riccati equation, and obtaining an initial expected acceleration u by the nonnegative definite constant solution P * (t)：
Will accelerate the speed u * (t) as the output of the LQR regulator.
S3, outputting the initial expected acceleration u by the LQR regulator * (t) clipping to obtain the desired acceleration u a (t) calculating a desired acceleration u a (t) inputting into a PID controller.
Specifically, the clipping rule is as follows:
wherein u is max Is a preset maximum sum of an inertial navigation systemSpeed.
S4, the PID controller tracks the expected acceleration u a (t) for the desired acceleration u a (t) obtaining the difference with the sampled current acceleration of the motor, and calculating a control quantity u q (t) as output of the PID controller, the control quantity u q And (t) is the Q-axis control voltage of the motor under the rotating coordinate system.
Control quantity u q The calculation procedure of (t) is as follows:
where e (t) is the desired acceleration u a (t) the difference value of the current acceleration of the motor obtained by sampling, wherein t is the current moment, tau is the intermediate moment, kp is the proportional term coefficient of PID, ki is the integral term coefficient of PID, and kd is the differential term coefficient of PID.
S5, calculating and processing the control quantity u by the inertial navigation system through an SVPWM algorithm q And (t) controlling a modulation shaft of the inertial navigation equipment, and enabling the modulation shaft to send a command to the motor so as to correct the running state of the motor.
The SVPWM algorithm is a short-form space vector pulse width modulation algorithm, is a pulse width modulation wave generated by a specific switching mode consisting of six power switching elements of a three-phase power inverter, can enable the output current waveform to be as close to an ideal sine waveform as possible, has small harmonic component of the winding current waveform processed by the SVPWM algorithm, reduces the torque pulsation of a motor, enables a rotating magnetic field to be more approximate to a circle, greatly improves the utilization rate of the direct-current bus voltage, and is easier to realize digitization.
Compared with a common three-loop control structure, the method has the advantages that various parameters containing 4 sets of PID are required to be debugged, the workload is high, and readjustment is required after environment changes. The parameters to be debugged in the method provided by the invention only comprise: q, R matrix of the LQR regulator and kp, ki and kd parameters of the PID controller; the parameter quantity is small, the debugging workload is small, and the optimal control is realized aiming at the selected objective function.
The diagonal coefficient of the Q matrix is used for adjusting the cost item of the position and speed phase error, and the R matrix is used for adjusting the cost item of the control quantity. The Q, R matrix of the LQR regulator can be obtained theoretically through simulation, and the kp, ki and kd parameters of the PID controller can be obtained through real object debugging.
In summary, the present invention provides an inertial navigation modulation axis control method based on an LQR regulator, including:
s1, sampling the current angular speed and the current angular position of a motor, outputting the expected angular speed and the expected angular position by an inertial navigation system, respectively calculating the difference between the expected angular speed and the angle position and the sampled angular speed and angle position, and respectively inputting the two difference values serving as an angular speed error and an angular error into an LQR regulator.
And S2, generating and solving a Riccati equation by taking a state equation of the motor as a target function in the LQR regulator, and taking the initial expected acceleration of the motor as the output of the LQR regulator.
And S3, carrying out amplitude limiting on the initial expected acceleration output by the LQR regulator to obtain the expected acceleration, and inputting the expected acceleration into the PID controller.
And S4, the PID controller tracks the expected acceleration, the difference is obtained between the expected acceleration and the sampled current acceleration of the motor, and the control quantity is calculated and used as the output of the PID controller, wherein the control quantity is the Q-axis control voltage of the motor in a rotating coordinate system.
And S5, calculating the processing control quantity by the inertial navigation system through an SVPWM algorithm, controlling a modulation shaft of the inertial navigation equipment, and modulating the shaft to send a command to the motor so as to correct the running state of the motor.
In the present invention, the terms "mounting", "connecting", "fixing" and the like are used in a broad sense, for example, "connecting" may be a fixed connection, a detachable connection, or an integral connection; "coupled" may be direct or indirect through an intermediary. The specific meanings of the above terms in the present invention can be understood by those skilled in the art according to specific situations.
The steps in the invention can be adjusted, combined and deleted in sequence according to actual requirements.
Although the present application has been disclosed in detail with reference to the accompanying drawings, it is to be understood that such description is merely illustrative and not restrictive of the application of the present application. The scope of the present application is defined by the appended claims and may include various modifications, adaptations, and equivalents of the invention without departing from the scope and spirit of the application.
Claims (5)
1. An inertial navigation modulation axis control method based on an LQR regulator is characterized by comprising the following steps:
s1, sampling the current angular speed and the current angular position of a motor, outputting the expected angular speed and the expected angular position by an inertial navigation system, respectively calculating the difference between the expected angular speed and the expected angular position and the sampled angular speed and the sampled angular position, and inputting the two difference values serving as an angular speed error and an angular error into an LQR regulator;
s2, generating and solving a Riccati equation by taking a state equation of the motor as a target function in the LQR regulator, and taking the initial expected acceleration of the motor as the output of the LQR regulator;
s3, carrying out amplitude limiting on the initial expected acceleration output by the LQR regulator to obtain expected acceleration, and inputting the expected acceleration into a PID controller;
s4, the PID controller tracks the expected acceleration, the difference between the expected acceleration and the sampled current acceleration of the motor is obtained, and a control quantity u is calculated q (t) as output of the PID controller, the control quantity u q (t) is the Q-axis control voltage of the motor under a rotating coordinate system;
s5, calculating and processing the control quantity u by the inertial navigation system through an SVPWM algorithm q And (t) controlling a modulation shaft of the inertial navigation equipment, and enabling the modulation shaft to send a command to the motor so as to correct the running state of the motor.
2. The LQR regulator-based inertial navigation modulation axis control method of claim 1, wherein the state equation of the motor is expressed by a vector equation as:
wherein e θ As an angle error, e ω For angular velocity error, u (t) is input magnitude acceleration, and t is time.
3. The LQR regulator-based inertial navigation modulation axis control method of claim 1, wherein the Riccati equation is:
wherein
Expressing an nonnegative definite-constant solution of a Riccati equation, wherein A expresses a state transition matrix, B expresses an input matrix, Q is an error cost item matrix of the LQR regulator, and R is an input cost item matrix of the LQR regulator;
thereby obtaining an initial desired acceleration u * (t)：
Where x (t) is the error vector.
4. The LQR-regulator-based inertial navigation modulation axis control method according to claim 1, wherein the clipping rule of the clipping in step S3 is as follows:
wherein u is max Is the maximum acceleration preset by the inertial navigation system.
5. The LQR regulator-based inertial navigation modulation axis control method according to claim 1,the control quantity u q The calculation procedure of (t) is as follows:
where e (t) is the desired acceleration u a (t) the difference value of the current acceleration of the motor obtained by sampling, wherein t is the current moment, tau is the intermediate moment, kp is the proportional term coefficient of PID, ki is the integral term coefficient of PID, and kd is the differential term coefficient of PID.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211537766.4A CN115903507A (en) | 2022-12-01 | 2022-12-01 | Inertial navigation modulation axis control method based on LQR regulator |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211537766.4A CN115903507A (en) | 2022-12-01 | 2022-12-01 | Inertial navigation modulation axis control method based on LQR regulator |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115903507A true CN115903507A (en) | 2023-04-04 |
Family
ID=86470882
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211537766.4A Pending CN115903507A (en) | 2022-12-01 | 2022-12-01 | Inertial navigation modulation axis control method based on LQR regulator |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115903507A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116700372A (en) * | 2023-07-04 | 2023-09-05 | 佛山市兴颂机器人科技有限公司 | Servo all-in-one machine speed control method based on improved PI algorithm |
-
2022
- 2022-12-01 CN CN202211537766.4A patent/CN115903507A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116700372A (en) * | 2023-07-04 | 2023-09-05 | 佛山市兴颂机器人科技有限公司 | Servo all-in-one machine speed control method based on improved PI algorithm |
| CN116700372B (en) * | 2023-07-04 | 2024-03-22 | 广东兴颂科技有限公司 | Servo all-in-one machine speed control method based on improved PI algorithm |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Mao et al. | Design and implementation of continuous finite-time sliding mode control for 2-DOF inertially stabilized platform subject to multiple disturbances | |
| CN107612445B (en) | Control method of follow-up speed regulation system with load acceleration feedback | |
| CN111585475B (en) | Brushless direct current motor servo system disturbance suppression and high-precision tracking control method | |
| CN105763119A (en) | Control system and control method for CMG framework permanent magnet synchronous motor | |
| CN112271970B (en) | Permanent magnet synchronous motor vector control method, equipment and storage medium | |
| CN115903507A (en) | Inertial navigation modulation axis control method based on LQR regulator | |
| CN113489403B (en) | Multi-motor load sharing cooperative control device and method for shield machine driving system | |
| Oukaci et al. | Comparison performance between sliding mode control and nonlinear control, application to induction motor | |
| CN113839589A (en) | Decoupling linear active disturbance rejection control method of permanent magnet synchronous motor | |
| Xia et al. | Optimal online resonance suppression in a drive system based on a multifrequency fast search algorithm | |
| Moreno-Valenzuela et al. | Lyapunov-based adaptive control for the permanent magnet synchronous motor driving a robotic load | |
| Lin et al. | Active disturbance rejection explicit model predictive direct speed control for permanent magnet synchronous motors | |
| CN108155837B (en) | time delay obtaining method and device for permanent magnet motor control system | |
| CN110995098B (en) | Inversion control method of permanent magnet synchronous motor | |
| CN113645399A (en) | Method and device for determining running position of holder equipment | |
| Li et al. | Double loop DR-PID control of a rotary inverted pendulum | |
| CN116247990B (en) | Voltage vector feedforward compensation method for permanent magnet synchronous motor | |
| Emelyanov et al. | Investigation of the dynamics of the manipulator drive with a stepper motor | |
| CN108494305A (en) | A kind of motor accelerating method, device, electron speed regulator and unmanned vehicle | |
| Bogosyan et al. | Robust-adaptive linearization with torque ripple minimization for a PMSM driven single link arm | |
| CN113285645B (en) | Motor control method, motor drive device, control method for industrial robot, and industrial robot | |
| CN113783478B (en) | Cascade dead beat control method and system for permanent magnet synchronous motor | |
| CN113285644B (en) | Motor control method, motor drive device, control method for industrial robot, and industrial robot | |
| CN111487860B (en) | High-precision rotating speed control method for large-inertia low-speed load | |
| KR101306846B1 (en) | A speed and torque optimal control apparatus for motor-based mechanism |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |