CN112905953A - Unscented Kalman filtering control method for inverted pendulum - Google Patents
Unscented Kalman filtering control method for inverted pendulum Download PDFInfo
- Publication number
- CN112905953A CN112905953A CN202110303714.XA CN202110303714A CN112905953A CN 112905953 A CN112905953 A CN 112905953A CN 202110303714 A CN202110303714 A CN 202110303714A CN 112905953 A CN112905953 A CN 112905953A
- Authority
- CN
- China
- Prior art keywords
- inverted pendulum
- state
- covariance
- matrix
- model
- 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
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000001914 filtration Methods 0.000 title claims abstract description 16
- 238000005070 sampling Methods 0.000 claims abstract description 37
- 230000009466 transformation Effects 0.000 claims abstract description 24
- 238000009826 distribution Methods 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 41
- 238000005259 measurement Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 10
- 230000003190 augmentative effect Effects 0.000 claims description 7
- 150000001875 compounds Chemical class 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 230000026676 system process Effects 0.000 claims description 3
- 230000004044 response Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 7
- 230000033001 locomotion Effects 0.000 description 6
- 230000003416 augmentation Effects 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000002068 genetic effect Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000012443 analytical study Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000739 chaotic effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000003032 molecular docking Methods 0.000 description 1
- 230000009022 nonlinear effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Abstract
The invention discloses an unscented Kalman filtering control method for an inverted pendulum, which comprises the following steps: firstly, initializing the mean value and the variance of the original state distribution of the inverted pendulum, then determining a sampling rule, then determining some Sigma points in the original state distribution according to the selected rule, enabling the determined Sigma points to be equal to the mean value and the covariance of the original state distribution, then carrying out nonlinear transformation on the Sigma points through a nonlinear system function to obtain a series of corresponding point sets, and finally determining the state mean value and the covariance after the nonlinear transformation by using the point sets obtained through the transformation. The invention solves the problems of slightly poor response time, control precision and robustness and limited active disturbance rejection capability of the existing inverted pendulum control algorithm.
Description
Technical Field
The invention belongs to the technical field of inverted pendulum control, and particularly relates to an unscented Kalman filtering control method for an inverted pendulum.
Background
Initial analytical studies of inverted pendulum systems began in the fifties of the twentieth century and were relatively complex, unstable, multivariable, high-order mechanical systems with nonlinear and strongly coupled characteristics. The inverted pendulum system has serious uncertainty, namely, the uncertainty of parameters of the system, and the interference of uncertain factors to the system. In recent years, many experts and scholars at home and abroad always regard the inverted pendulum as a typical research object, a plurality of control schemes are provided, a great deal of research is carried out on the stability and the stability of the inverted pendulum system, different control methods are sought to realize the control of the inverted pendulum so as to check or explain the control capability of the severely nonlinear and absolutely unstable system of the method, and the control method has wide application in the fields of military industry, aerospace, robots and general industrial processes, such as the processing of precision instruments, the balance control of the walking process of the robots, the verticality control in rocket launching, the missile interception control, the aviation docking control, the attitude control in satellite flight and the like. Therefore, from the viewpoint of control, the research on the inverted pendulum has far-reaching significance in theory and methodology. An inverted pendulum system is a typical self-unstable system in which the pendulum, as a typical vibration and motion problem, can be abstracted into many problems to study. With the development of nonlinear science, the nonlinear method is used for describing the nonlinear property, which is naturally unquestionable, but the method has a limitation that some essential characteristics of the nonlinearity are not always embodied by the linear method. Nonlinearity is a core factor causing chaos, disorder or chaos, which does not mean a complex reason, and simple nonlinearity can generate very chaos, disorder or chaos. The inverted pendulum system contains extremely rich and complex dynamics behaviors, such as bifurcation, fractal and chaotic dynamics.
In the prior art, the control of the inverted pendulum has classical, modern and various intelligent control theories, such as LQR control, fuzzy control, genetic algorithm control, sliding mode variable structure control and the like, but the influence of system noise, measurement noise, external interference and modeling error is difficult to avoid in the implementation process of various controls, so that the designed controller has limited anti-interference capability. In recent years, there are control laws designed by using KF, EKF, and the like, and although the interference rejection capability is enhanced, KF needs to calculate a state transition matrix in a linearized manner, which causes model errors. The EKF performs first-order linear approximation on the nonlinear system, and then performs Kalman filtering processing on the system, thereby achieving the purpose of expanding the Kalman filtering theory to the field of the nonlinear system. However, the EKF method only takes the first-order part of the Taylor-series expansion of the nonlinear system to perform linear approximation, so that approximation errors are inevitably generated, which is not suitable in an environment with higher precision requirement.
Disclosure of Invention
In order to solve the problems, the invention discloses an unscented Kalman filtering control method for an inverted pendulum, which solves the problems of slightly poor response time, control precision and robustness and limited active disturbance rejection capability of the existing inverted pendulum control algorithm.
The specific scheme is as follows:
an unscented Kalman filtering control method for an inverted pendulum is characterized by comprising the following steps: firstly, initializing the mean value and the variance of the original state distribution of the inverted pendulum, then determining a sampling rule, then determining some Sigma points in the original state distribution according to the selected rule, enabling the determined Sigma points to be equal to the mean value and the covariance of the original state distribution, carrying out nonlinear transformation on the Sigma points through a nonlinear system function to obtain a series of corresponding point sets, and finally determining the state mean value and the covariance after the nonlinear transformation by using the point sets obtained through the transformation.
The method comprises the following basic steps of unscented transformation based on a state variance matrix diagonal similarity decomposition sampling strategy: given the nonlinear state transformation y ═ f (x), where the state vector x is an n-dimensional random vector, assuming the system state mean is
If the variance is P, the steps of obtaining the Sigma point and the corresponding weight by UT conversion are as follows:
(1) 2n +1 sampling points are obtained through the following formula, wherein n is the dimension of the system model state;
wherein D ═ diag (ζ)1,ζ2,ζ3,...,ζn)，ζi(i ═ 1, 2.. and n) is a diagonal matrix composed of characteristic values of P, (P ═ Pm)iDenotes the ith column, f of the matrixeigThe function is to solve all characteristic values of the matrix P to form a diagonal matrix D, and solve the characteristic vector of P to form a column vector of H;
(2) solving a corresponding weighting factor of a Sigma point of the model state;
in the formula (I), the compound is shown in the specification,
is the state mean weighting factor for the initial sample point,
is the covariance weighting factor of the initial sample point,
and
state mean and covariance weighting factors corresponding to the ith sampling point; beta ≧ 0 is a nonnegative weight coefficient used to incorporate higher-order term moments in the system equation, and the presence of beta can include the effects of the higher-order terms in the system [26 ]](optimal for the case of gaussian distribution β -2); alpha is a main scale parameter for determining the distribution breadth of Sigma points near the prior mean value, and the value range of the alpha is generally 10-4Alpha is not less than 1, and the distribution state of sampling points can be controlled by taking different alpha values; k is a candidate parameter without specific values and boundaries, but for symmetric sampling, the value of k is usually to ensure that the matrix (n + λ) P is a semi-positive definite matrix, and usually k is 0 or 3-n; λ ═ α2(n + κ) -n is a scaling parameter used to reduce the overall prediction error of the algorithm.
Discretizing the nonlinear continuous model of the inverted pendulum, and selecting zero-mean white Gaussian noiseFor the system process noise W (k) and the measurement noise V (k), the state vector and the observation vector after the known inverted pendulum is augmented are xeAnd z, the discretized inverted pendulum nonlinear identification system model can be described by equations (3.3) and (3.4),
in the formula (f)eRepresents the nonlinear process function h of the inverted pendulum model after the state is expandedeAn observation function of the inverted pendulum is shown.
Assuming that process noise W (k) and measurement noise V (k) in the inverted pendulum model respectively have covariance matrices Q and R, at different time k, the model augmentation state vector x for the inverted pendulumeThe unscented Kalman filtering algorithm is basically realized by the following steps:
(1) determining the state sampling points (namely a Sigma point set) of the inverted pendulum model by the following formula;
(2) performing one-step prediction on the acquired sampling points through a nonlinear transformation function;
(3) calculating a one-step prediction mean and covariance matrix by first calculating a weighting factor using equation (3.2)
And
then, carrying out weighted summation on the one-step predicted values of the sampling points to obtain the mean value and covariance of the one-step prediction of the inverted pendulum state;
(4) performing UT conversion on the one-step prediction average value of the inverted pendulum state obtained in the step (3) to obtain a new sampling point;
(5) updating a measured value, namely performing measurement transformation on the sampling point set obtained in the previous step to further obtain an observation predicted value of the inverted pendulum state;
(6) calculating the mean value and the covariance of the observation predicted values, weighting the observation predicted values of the inverted pendulum sampling points obtained in the previous step by weighting factors, and then solving the corresponding mean value and covariance;
in the formula (I), the compound is shown in the specification,
is the observation variance of the inverted pendulum system model;
is the state observation covariance of the inverted pendulum system model;
(7) calculating a corresponding Kalman gain matrix by using the covariance matrix obtained in the previous step;
(8) updating the system state quantity and covariance;
and at this time, one-time updating of the state mean value and covariance of the inverted pendulum model is completed.
The invention has the beneficial effects that: according to the method, the device and the system for controlling the inverted pendulum based on unscented Kalman filtering, the unscented Kalman filtering algorithm is adopted, compared with the traditional inverted pendulum control algorithm, unscented Kalman filtering is adopted in the controller, the UKF is used for directly estimating the state of the nonlinear system, the system does not need to be subjected to linearization processing, and the method, the device and the system have more ideal control effect and anti-interference performance. And a state variance matrix diagonal similarity decomposition strategy is adopted, so that the problems that the UKF algorithm cannot be converged, the calculation load is large, the state variance matrix cannot keep positive definite and the like are solved. The motion attitude prediction of the swing rod is more accurate, the inertia and interference influence of the inverted pendulum is reduced, and the stability of the swing rod is kept. Effectively reduce inertia and interference influence, whole device has higher reliability and robustness, simple structure, low cost.
Drawings
FIG. 1 is a diagram of a classical control principle of an inverted pendulum.
FIG. 2 is a simplified second-order system block diagram of the inverted pendulum classical control.
Fig. 3 shows the modern control principle of the inverted pendulum.
FIG. 4 is a flow chart of predicting the inverted pendulum state based on unscented Kalman filtering according to the present invention.
Fig. 5 is a general configuration diagram of an XZ-iia type rotary inverted pendulum system.
Fig. 6 is a block diagram of the XZ-iia type rotary inverted pendulum system.
FIG. 7 is a kinetic model diagram.
Detailed Description
The present invention will be further illustrated with reference to the accompanying drawings and specific embodiments, which are to be understood as merely illustrative of the invention and not as limiting the scope of the invention.
As shown in fig. 1-3, the prior art is directed to classical control of an inverted pendulum: the rotary arm is driven by a direct current torque motor at the rotating shaft and can rotate around the rotating shaft in a vertical plane vertical to the rotating shaft of the motor; the rotary arm and the swing rod are connected by a movable rotating shaft of the potentiometer, and the swing rod can rotate around the rotating shaft in a vertical plane vertical to the rotating shaft; 2 angular displacement signals (the included angle between the swing arm and a lead straight line and the relative angle between the swing rod and the swing arm) obtained by measurement of the potentiometer are taken as 2 output quantities of the system and are sent to the DSP controller; an angular velocity signal can be obtained through the difference of angular displacement, then a PWM control signal is regulated according to a PID regulation algorithm and is converted into a voltage signal to be supplied to a driving circuit so as to drive the movement of a direct-current torque motor, and the movement of the swing rod is controlled by the rotation of the swing arm driven by the motor. Modern control (control based on state feedback): firstly, the system is judged to be controllable and can be observed, then a feedback control law is determined according to state feedback, and a feedback matrix K is solved by using a pole allocation method, so that the closed loop of the system is stable. Intelligent control: a control law is designed based on intelligent optimization algorithms such as an ant colony algorithm, a genetic algorithm, a neural network and the like, so that the inverted pendulum system is stable in closed loop.
As shown in fig. 4, the invention provides an unscented kalman filter control method for an inverted pendulum, comprising the following steps: firstly, initializing the mean value and the variance of the original state distribution of the inverted pendulum, then determining a sampling rule, then determining some Sigma points in the original state distribution according to the selected rule, enabling the determined Sigma points to be equal to the mean value and the covariance of the original state distribution, carrying out nonlinear transformation on the Sigma points through a nonlinear system function to obtain a series of corresponding point sets, and finally determining the state mean value and the covariance after the nonlinear transformation by using the point sets obtained through the transformation.
The method comprises the following basic steps of unscented transformation based on a state variance matrix diagonal similarity decomposition sampling strategy: given the nonlinear state transformation y ═ f (x), where the state vector x is an n-dimensional random vector, assuming the system state mean is
If the variance is P, the steps of obtaining the Sigma point and the corresponding weight by UT conversion are as follows:
(1) 2n +1 sampling points are obtained through the following formula, wherein n is the dimension of the system model state;
wherein D ═ diag (ζ)1,ζ2,ζ3,...,ζn)，ζi(i ═ 1, 2.. and n) is a diagonal matrix composed of characteristic values of P, (P ═ Pm)iDenotes the ith column, f of the matrixeigThe function is to solve all characteristic values of the matrix P to form a diagonal matrix D, and solve the characteristic vector of P to form a column vector of H;
(2) solving a corresponding weighting factor of a Sigma point of the model state;
in the formula (I), the compound is shown in the specification,
is the state mean weighting factor for the initial sample point,
is the covariance weighting factor of the initial sample point,
and
state mean and covariance weighting factors corresponding to the ith sampling point; beta ≧ 0 is a nonnegative weight coefficient used to incorporate higher-order term moments in the system equation, and the presence of beta can include the effects of the higher-order terms in the system [26 ]](optimal for the case of gaussian distribution β -2); alpha is a main scale parameter for determining the distribution breadth of Sigma points near the prior mean value, and the value range of the alpha is generally 10-4Alpha is not less than 1, and the distribution state of sampling points can be controlled by taking different alpha values; k is a candidate parameter without specific values and boundaries, but for symmetric sampling, the value of k is usually to ensure that the matrix (n + λ) P is a semi-positive definite matrix, and usually k is 0 or 3-n; λ ═ α2(n + κ) -n is a scaling parameter used to reduce the overall prediction error of the algorithm.
Discretizing a nonlinear continuous model of the inverted pendulum, selecting zero-mean white Gaussian noise as system process noise W (k) and measurement noise V (k), wherein the state vector and the observation vector of the inverted pendulum after being augmented are respectively known as xeAnd z, the discretized inverted pendulum nonlinear identification system model can be described by equations (3.3) and (3.4),
in the formula (f)eRepresents the nonlinear process function h of the inverted pendulum model after the state is expandedeAn observation function of the inverted pendulum is shown.
Assuming that process noise W (k) and measurement noise V (k) in the inverted pendulum model respectively have covariance matrices Q and R, at different time k, the model augmentation state vector x for the inverted pendulumeThe unscented Kalman filtering algorithm is basically realized by the following steps:
(1) determining the state sampling points (namely a Sigma point set) of the inverted pendulum model by the following formula;
(2) performing one-step prediction on the acquired sampling points through a nonlinear transformation function;
(3) calculating a one-step prediction mean and covariance matrix by first calculating a weighting factor using equation (3.2)
And
then, carrying out weighted summation on the one-step predicted values of the sampling points to obtain the mean value and covariance of the one-step prediction of the inverted pendulum state;
(4) performing UT conversion on the one-step prediction average value of the inverted pendulum state obtained in the step (3) to obtain a new sampling point;
(5) updating a measured value, namely performing measurement transformation on the sampling point set obtained in the previous step to further obtain an observation predicted value of the inverted pendulum state;
(6) calculating the mean value and the covariance of the observation predicted values, weighting the observation predicted values of the inverted pendulum sampling points obtained in the previous step by weighting factors, and then solving the corresponding mean value and covariance;
in the formula (I), the compound is shown in the specification,
is the observation variance of the inverted pendulum system model;
is the state observation covariance of the inverted pendulum system model;
(7) calculating a corresponding Kalman gain matrix by using the covariance matrix obtained in the previous step;
(8) updating the system state quantity and covariance;
and at this time, one-time updating of the state mean value and covariance of the inverted pendulum model is completed.
Examples
An XZ-IIA type rotary inverted pendulum system is a typical electromechanical integrated system, and a motion controller and a servo motor are adopted to carry out real-time motion control. The XZ-IIA type rotary inverted pendulum system is directly driven by a direct current torque motor and controlled by a built-in DSP chip. The device can be directly operated without a computer, and can also be controlled by the computer through serial port communication. Fig. 5 is a general configuration diagram of an XZ-iia type rotary inverted pendulum system. Fig. 6 is a block diagram of the XZ-iia type rotary inverted pendulum system. The system comprises a computer, a DSP controller, a driving mechanism, an inverted pendulum body and a position detection element, and forms a closed loop system.
The TMS320F240 DSP controller is adopted as a core device in the system, a real-time control algorithm can be independently executed, and the system can also be communicated with a computer through an RS-232C serial bus to debug the on-line control algorithm. Its working principle is that 2 angular displacement signals (included angle between rotary arm and lead straight line and relative angle between swing rod and rotary arm) obtained by potentiometer measurement are used as 2 output quantities of system and fed into computer. The computer calculates the control quantity according to a certain control algorithm, converts the control quantity into a corresponding voltage signal and provides the voltage signal for the driving circuit so as to drive the direct current torque motor to move, and controls the inversion of the swing rod and keeps balance by driving the rotation of the swing arm through the motor.
After the theoretical analysis of the inverted pendulum system ignores various influences such as friction and the like, a dynamic model shown in fig. 7 can be abstracted. The main mechanical parameters and variables of the system are shown in the following table.
The swing arm rotates around the shaft, the moment of inertia J1 and the corresponding friction moment coefficient f1 of the swing arm rotate around the shaft, and the moment of inertia J2 and the corresponding friction moment coefficient f2 of the swing rod rotate around the shaft. By measuring and calculating J1 ═ 0.004Kg · m2,J2＝0.001Kg·m2F1 is 0.01N · m · s, and f2 is 0.001N · m · s. Theta 1 and theta 2 are respectively included angles between the spiral arm and the swing rod and a vertical line, and the clockwise direction is positive; u is the control voltage applied to the motorThe clockwise direction is positive and the counterclockwise direction is negative. The system nonlinear model is as follows:
let θ 1 → 0, θ 2 → 0, then the linearized model is:
order to
Then there is
The state space equation for the system is as follows:
Since rank [ B, AB, A2B, A3B ] ═ 4, rank [ C; CA; CA 2; CA3 ═ 4, the system is fully controllable and fully observable.
The corresponding inverted pendulum UKF state prediction algorithm is designed by the following main steps:
(1) UKF parameter and state quantity initialization
Firstly, the corresponding state variables are initialized by the formula (3.2)And observed quantity weighting factor
And variance weighting factor
And an associated scale factor lambda. Assuming that the process noise and the measurement noise are white gaussian noise with zero mean, wherein the process noise variance matrix is Q and the measurement noise variance matrix is R, the initialization values of the inverted pendulum augmented state vector and the variance thereof are shown in formulas (3.17) and (3.18).
In the formula (I), the compound is shown in the specification,
initializing a mean, P, for an augmented state vector0Initializing variance for the corresponding; x is the number of0And η0Respectively are the initialization values of the basic state quantity and the augmentation state quantity of the inverted pendulum,
and
the covariance is initialized accordingly.
(2) Calculating Sigma Point and time updates
The UKF algorithm accomplishes the nonlinear transfer of random state variables through a certain set of samples. Let the external input signal of the nonlinear system of the inverted pendulum at the known k moment be deltakInverted pendulum model augmented state vector xeThe corresponding estimated value and the estimated error variance matrix are respectively
And
then the Sigma points of 2n +1 samples at time k and their predicted values, predicted mean and state covariance predicted values can be obtained by equations (3.5) - (3.8).
(3) Observed value update
After the calculation of the Sigma point and the time update of the state quantity are completed, the observed quantity of the inverted pendulum needs to be updated. The system observation predicted value of the Sigma point is z at the moment kk|kThe observed predicted mean value is
The corresponding observed prediction variance matrix is
The covariance matrix of the state quantity and observation quantity predictions is
Kalman filter gain of Kk+1The state variance matrix at time k +1 is Pk+1The state estimation value is
The state estimate and covariance matrix of the system can be iteratively updated by equations (3.10) - (3.16).
Selecting state quantity of system
As observed quantities, an observation equation of the object to be studied is established as:
the technical means disclosed in the invention scheme are not limited to the technical means disclosed in the above embodiments, but also include the technical scheme formed by any combination of the above technical features. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and such improvements and modifications are also considered to be within the scope of the present invention.
Claims (3)
1. An unscented Kalman filtering control method for an inverted pendulum is characterized by comprising the following steps:
firstly, initializing the mean value and the variance of the original state distribution of the inverted pendulum, then determining a sampling rule, then determining some Sigma points in the original state distribution according to the selected rule, enabling the determined Sigma points to be equal to the mean value and the covariance of the original state distribution, then carrying out nonlinear transformation on the Sigma points through a nonlinear system function to obtain a series of corresponding point sets, and finally determining the state mean value and the covariance after the nonlinear transformation by using the point sets obtained through the transformation.
2. The unscented kalman filter control method of the inverted pendulum according to claim 1, characterized in that the unscented transformation basic steps of the sampling strategy based on the state variance matrix diagonal similarity decomposition are as follows:
given the nonlinear state transformation y ═ f (x), where the state vector x is an n-dimensional random vector, assuming the system state mean is
If the variance is P, the steps of obtaining the Sigma point and the corresponding weight by UT conversion are as follows:
(1) 2n +1 sampling points are obtained through the following formula, wherein n is the dimension of the system model state;
wherein D ═ diag (ζ)1,ζ2,ζ3,...,ζn)，ζi(i ═ 1, 2.. and n) is a diagonal matrix composed of characteristic values of P, (P ═ Pm)iRepresents the ith column of the matrix，feigThe function is to solve all characteristic values of the matrix P to form a diagonal matrix D, and solve the characteristic vector of P to form a column vector of H;
(2) solving a corresponding weighting factor of a Sigma point of the model state;
in the formula (I), the compound is shown in the specification,
is the state mean weighting factor for the initial sample point,
is the covariance weighting factor of the initial sample point,
and
state mean and covariance weighting factors corresponding to the ith sampling point; beta is more than or equal to 0 and is a non-negative weight coefficient used for combining the moments of the high-order terms in the system equation, the influence of the high-order terms of the system can be included by the existence of beta, and the beta is 2 optimal for the situation of Gaussian distribution; alpha is a main scale parameter for determining the distribution breadth of Sigma points near the prior mean value, and the value range of the alpha is 10-4Alpha is not less than 1, and the distribution state of sampling points can be controlled by taking different alpha values; kappa is a parameter to be selected without specific values and limits, but for symmetric sampling, the value of the matrix (n + lambda) P is a semi-positive definite matrix, and the value of kappa is 0 or 3-n; λ ═ α2(n + κ) -n is a scaling parameter used to reduce the overall prediction error of the algorithm.
3. The unscented kalman filter control method of an inverted pendulum according to claim 1, characterized in that the nonlinearity for the inverted pendulumDiscretizing the continuous model, selecting zero-mean white Gaussian noise as system process noise W (k) and measurement noise V (k), wherein the state vector and observation vector after the known inverted pendulum is augmented are x respectivelyeAnd z, the discretized inverted pendulum nonlinear identification system model can be described by equations (3.3) and (3.4),
in the formula (f)eRepresents the nonlinear process function h of the inverted pendulum model after the state is expandedeAn observation function representing the inverted pendulum;
assuming that process noise W (k) and measurement noise V (k) in the inverted pendulum model have covariance matrices Q and R respectively; augmenting the state vector x for the model of the inverted pendulum at different times keThe unscented Kalman filtering algorithm is basically realized by the following steps:
(1) determining a state sampling point of the inverted pendulum model, namely a Sigma point set, through the following formula;
(2) performing one-step prediction on the acquired sampling points through a nonlinear transformation function;
(3) calculating a one-step prediction mean and covariance matrix by first calculating a weighting factor using equation (3.2)
And
then, the one-step prediction value of the sampling point is weighted and summed to obtain the one-step prediction of the inverted pendulum stateMean and covariance;
(4) performing UT conversion on the one-step prediction average value of the inverted pendulum state obtained in the step (3) to obtain a new sampling point;
(5) updating a measured value, namely performing measurement transformation on the sampling point set obtained in the previous step to further obtain an observation predicted value of the inverted pendulum state;
(6) calculating the mean value and the covariance of the observation predicted values, weighting the observation predicted values of the inverted pendulum sampling points obtained in the previous step by weighting factors, and then solving the corresponding mean value and covariance;
in the formula (I), the compound is shown in the specification,
is the observation variance of the inverted pendulum system model;
is the state observation covariance of the inverted pendulum system model;
(7) calculating a corresponding Kalman gain matrix by using the covariance matrix obtained in the previous step;
(8) updating the system state quantity and covariance;
and at this point, one-time updating of the state mean value and covariance of the inverted pendulum model is completed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110303714.XA CN112905953A (en) | 2021-03-22 | 2021-03-22 | Unscented Kalman filtering control method for inverted pendulum |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110303714.XA CN112905953A (en) | 2021-03-22 | 2021-03-22 | Unscented Kalman filtering control method for inverted pendulum |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112905953A true CN112905953A (en) | 2021-06-04 |
Family
ID=76105885
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110303714.XA Pending CN112905953A (en) | 2021-03-22 | 2021-03-22 | Unscented Kalman filtering control method for inverted pendulum |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112905953A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113625568A (en) * | 2021-08-11 | 2021-11-09 | 上海交通大学 | Inverted pendulum passive self-adaptive sliding mode control method based on generalized system model |
| CN116182949A (en) * | 2023-02-23 | 2023-05-30 | 中国人民解放军91977部队 | Marine environment water quality monitoring system and method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101615794A (en) * | 2009-08-05 | 2009-12-30 | 河海大学 | Electrical Power System Dynamic method for estimating state based on no mark transformation Kalman filter |
| CN110912535A (en) * | 2019-12-11 | 2020-03-24 | 云南大学 | Novel pilot-free Kalman filtering method |
| CN111783243A (en) * | 2020-06-18 | 2020-10-16 | 东南大学 | Metal structure fatigue crack propagation life prediction method based on filtering algorithm |
| CN111780981A (en) * | 2020-05-21 | 2020-10-16 | 东南大学 | Intelligent vehicle formation lane change performance evaluation method |
-
2021
- 2021-03-22 CN CN202110303714.XA patent/CN112905953A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101615794A (en) * | 2009-08-05 | 2009-12-30 | 河海大学 | Electrical Power System Dynamic method for estimating state based on no mark transformation Kalman filter |
| CN110912535A (en) * | 2019-12-11 | 2020-03-24 | 云南大学 | Novel pilot-free Kalman filtering method |
| CN111780981A (en) * | 2020-05-21 | 2020-10-16 | 东南大学 | Intelligent vehicle formation lane change performance evaluation method |
| CN111783243A (en) * | 2020-06-18 | 2020-10-16 | 东南大学 | Metal structure fatigue crack propagation life prediction method based on filtering algorithm |
Non-Patent Citations (3)
| Title |
|---|
| 伍晓丽 等: "基于无迹卡尔曼滤波的旋转倒立摆LQR控制", 《测控技术》, vol. 36, no. 4, pages 93 - 96 * |
| 余扬 等: "基于无迹卡尔曼滤波的一级倒立摆双回路PID控制", 《电脑知识》, vol. 15, no. 3, pages 260 - 262 * |
| 虞培培 等: "无迹卡尔曼滤波算法在二级倒立摆上的应用研究", 《模糊系统与数学》, vol. 25, no. 5, pages 158 - 161 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113625568A (en) * | 2021-08-11 | 2021-11-09 | 上海交通大学 | Inverted pendulum passive self-adaptive sliding mode control method based on generalized system model |
| CN116182949A (en) * | 2023-02-23 | 2023-05-30 | 中国人民解放军91977部队 | Marine environment water quality monitoring system and method |
| CN116182949B (en) * | 2023-02-23 | 2024-03-19 | 中国人民解放军91977部队 | Marine environment water quality monitoring system and method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhou et al. | Deep convolutional neural network based fractional-order terminal sliding-mode control for robotic manipulators | |
| Rigatos | Particle filtering for state estimation in nonlinear industrial systems | |
| Long et al. | A vibration control method for hybrid-structured flexible manipulator based on sliding mode control and reinforcement learning | |
| CN112905953A (en) | Unscented Kalman filtering control method for inverted pendulum | |
| Meng et al. | Aerodynamic parameter estimation of an unmanned aerial vehicle based on extended kalman filter and its higher order approach | |
| CN109343550B (en) | Spacecraft angular velocity estimation method based on rolling time domain estimation | |
| CN111813110B (en) | Active disturbance rejection control method for path following of snake-shaped robot | |
| Gao et al. | Neural network supervision control strategy for inverted pendulum tracking control | |
| CN115157238A (en) | Multi-degree-of-freedom robot dynamics modeling and trajectory tracking method | |
| CN113377006B (en) | Global fast terminal sliding mode control method based on invariant flow observer | |
| Qin et al. | Dual-loop robust attitude control for an aerodynamic system with unknown dynamic model: Algorithm and experimental validation | |
| Chen et al. | Robust sliding-mode tip position control for flexible arms | |
| Komeno et al. | Deep koopman with control: Spectral analysis of soft robot dynamics | |
| Ding et al. | Approximate dynamic programming solutions with a single network adaptive critic for a class of nonlinear systems | |
| Zhou et al. | Launch vehicle adaptive flight control with incremental model based heuristic dynamic programming | |
| CN113954077B (en) | Underwater swimming mechanical arm trajectory tracking control method and device with energy optimization function | |
| Chen et al. | Adaptive Control of Robot Manipulators in Varying Environments | |
| Xin et al. | Partial model-free control of a 2-input and 2-output helicopter system | |
| Guerra et al. | Decentralized neural block control for a robot manipulator based in ukf training | |
| Zhang et al. | MPC for 3-D Trajectory Tracking of UUV with Constraints Using Laguerre Functions | |
| Xu et al. | Tracking Control of Wheeled Mobile Robot Based on MMPC | |
| Gui et al. | Adaptive parameter estimation and velocity control of uav systems | |
| Yang et al. | Closed-Loop Subspace Predictive Control of Gyroscope | |
| Zhu et al. | A Fault Diagnosis Method for Satellite Reaction Wheel Based on PSO-ELM | |
| Ye et al. | A data-driven optimal time-delayed control approach and its application to aerial manipulators |
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 |