Inverted pendulum control method, device and system based on EKF and LPF compounding

Abstract

The invention discloses an inverted pendulum control method, device and system based on EKF and LPF compounding, a sensor collects swing rod angle information and trolley position information, the data are transmitted to a low-pass filter (LPF) for processing, the processed data are embedded into extended Kalman filter (EKF) for iterative optimization processing, and the data are closer to reality; the core controller calculates the state according to the feedback gain and the data processed by the composite filter, outputs the voltage of the direct current motor, and controls the inverted pendulum system. In the process, the core controller carries out iterative optimization estimation on state input variables of collected data through EKF and LPF composite filtering, interference of random noise and high-frequency vibration noise of the inverted pendulum system on the state data of the inverted pendulum system is weakened, and the control effect of the controller is guaranteed. According to the invention, the problems of instability, loud noise and the like of input data are solved, so that the advantages and disadvantages of different control strategies are researched.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical control, in particular to an inverted pendulum control method, device and system based on EKF and LPF combination. BACKGROUND

[0002] As an inverted pendulum has the characteristics of multi-coupling, multi-variable, single output, strong coupling, and involves nonlinear behavior and absolute instability, and due to the simple structure, low cost and easily observed parameters of the inverted pendulum, the phenomenon is obvious, and the algorithm is easy to model, the inverted pendulum experiment platform can well demonstrate the stability, controllability and observability of the control system, and is widely used as an algorithm verification platform by researchers in the control engineering field. At the same time, the research on inverted pendulum algorithm not only has theoretical and teaching significance, but also has similar applications in rocket launch, missile interception, unmanned driving, humanoid robot gait control and various industrial fields.

[0003] However, when the inverted pendulum collects sensor data, there is serious high-frequency vibration noise interference and random noise interference, and the data with interference seriously affects the performance of the control algorithm, and even causes the control algorithm to distort, so filtering the collected data is very important. SUMMARY

[0004] In order to solve the above problems, the present application discloses an inverted pendulum control method, device and system based on EKF and LPF combination, which solves the problem of serious noise interference of sensor collected data, resulting in control failure, poor system stability, poor robustness and poor anti-interference ability.

[0005] The specific scheme is as follows: According to the first aspect of the present application, an inverted pendulum control method based on EKF and LPF combination is provided, comprising: After the inverted pendulum system is powered on, the pendulum rod angle data is collected, the average value in the group of data is taken, and the average value is taken as the zero offset value Recorded down; According to the sampling frequency The motor position , In order to eliminate the value after zero offset, the inverted pendulum rod angle , respectively through Fourier transform, the frequency spectrum is obtained, and the high-frequency vibration frequency is found out ; Use the collection frequency And the high-frequency vibration frequency Make a pre-low-pass filter; Low-pass filter: The low-pass filter is used to filter out part of the high-frequency noise and get the smooth pendulum angle data and the position data of the pendulum carrier trolley , and the pendulum angular velocity is calculated according to the sampling frequency and the speed of the pendulum carrier trolley , , is the low-pass filter coefficient; According to the inverted pendulum system and the pre low-pass filter, a nonlinear dynamic model is established and converted into a discrete state space equation: wherein is the moment of inertia of the motor shaft, is the mass of the pendulum, is the total mass of the pendulum carrier trolley, is the length of the pendulum, is the friction coefficient of the pendulum carrier trolley, is the rotational friction coefficient of the pendulum, is the time interval between two samplings, is the force output by the motor, is the spatial state vector, is the spatial observation vector; The DC motor model is also included in the spatial state equation, and the output voltage value is: wherein is the back electromotive force coefficient, is the electromagnetic torque coefficient, is the armature resistance, is the synchronous pulley radius, is the moment of inertia of the synchronous pulley rotating around the motor shaft, is the output voltage value; The state observer and EKF and LPF compound filter are introduced to further remove random noise, and the formula is: wherein is the normally distributed system noise consistent with , is the normally distributed observation noise consistent with ; Jacobian matrix: Extended Kalman filter: wherein , is the Kalman gain, is the inverse matrix of is the true value of the collected data; The probability distribution of the measured value and the true value of the collected data obeys the normal distribution , let ; Let , solve where is the prior covariance matrix, is the covariance matrix of the measurement error ; The update formula of the prior covariance matrix : The update formula of the posterior covariance matrix : At each step, find and then calculate the posterior value ; Use LQR and optimal control theory to find the optimal gain: where is the terminal cost, is the input weight, is the output weight, is the minimum cost value; According to the dynamic programming principle: where is the cost value generated in the first step, is the minimum cost generated in the first step, let , , the minimum output from the first step to the first step is , let , that is into , is the output weight; We can get: where , the iterative relationship can be obtained from the above derivation: At this time, the optimal control law is , Feedback gain constant vector ; the data collected by the sensor is filtered through the composite filter of the pre-low pass filter and the extended Kalman filter, and the filtered data is combined with the optimal gain to output the voltage value, the controller outputs the PWM value according to the voltage value, and the inverted pendulum rod is controlled to be stable.

[0006] According to the second aspect of the present application, an inverted pendulum control device based on the composite of EKF and LPF is provided, comprising: A position and angle information acquisition module is used to acquire the inverted pendulum angle information And the inverted pendulum rod carrier trolley position information ; A pre-low pass filter and extended Kalman filter composite filter module is used to smooth the collected data through the pre-low pass filter, reduce noise, and obtain the inverted pendulum angle information And the trolley position information , Kalman state estimation: , And The state space equation is introduced into the Jacobian matrix linearization to obtain the prior state vector , The Kalman gain And the state transition Jacobian matrix The product is fused into Gaussian noise processing to obtain the collected data with reduced Gaussian noise. A state judgment module is used to judge the current motion state according to the current swing rod angle And the trolley position ; A calculation output module: According to the target state vector , The optimal feedback gain Through the composite filter of the low pass filter and the extended Kalman filter and the optimal control theory, the current output voltage value is calculated, and the voltage value is converted into a PWM value by the controller to output to the motor.

[0007] According to the third aspect of the present application, an inverted pendulum control system based on the composite of EKF and LPF is provided, comprising the inverted pendulum control device based on the composite of EKF and LPF, and the inverted pendulum control device based on the composite of EKF and LPF comprises a memory and a processor, wherein the processor is used to execute the control command stored in the memory, judge the motion state, and execute the inverted pendulum control method based on the composite of EKF and LPF and the corresponding motion instruction.

[0008] The inverted pendulum control system based on low-pass filtering further comprises a motor, an inverted pendulum carrier trolley, an inverted pendulum guide rail, a synchronous belt, a synchronous pulley, an angle potentiometer, an encoder and a swing rod. The angle potentiometer is fixed on the trolley carrier, and the shaft of the angle potentiometer is connected with the swing rod. The encoder is installed on the motor shaft and used for collecting the rotation angle of the motor output, and the other end of the motor shaft is connected with the synchronous pulley. The synchronous belt is connected with the synchronous pulley and used for transmitting the power output by the motor to control the movement of the swing rod. The PWM output port of the control device is connected with the drive board of the motor and used for controlling the forward and reverse rotation and the rotation speed of the motor.

[0009] According to a fourth aspect of the present application, an inverted pendulum device is provided, comprising the inverted pendulum control system based on EKF and LPF.

[0010] The inverted pendulum control method, system and device based on EKF and LPF provided by the present application adopt a front low-pass filter and an extended Kalman filter composite filter, compared with the traditional low-pass filter, the low-pass filter is embedded in the extended Kalman filter for iterative optimization, not only the high-frequency noise is weakened, but also the influence of process noise, Gaussian noise and random noise is weakened, so that the data is closer to the true value, the control effect of the control strategy is effectively improved, the problems of unstable input data and large noise are solved, so as to facilitate the research on the advantages and disadvantages of different control strategies, and the device has the characteristics of low cost and simple device. BRIEF DESCRIPTION OF DRAWINGS

[0011] Figure 1 is a schematic diagram of the inverted pendulum device based on EKF and LPF of the embodiment of the present application; Figure 2 is a control system composition diagram of the embodiment of the present application; Figure 3 is a block diagram of the control system of the embodiment of the present application; DETAILED DESCRIPTION

[0012] The present application will be further described in detail below in combination with the drawings and specific embodiments: EMBODIMENT

[0013] An inverted pendulum control method based on EKF and LPF, comprising: A position sensor is used to acquire the position of the trolley each time sampling: ; With angle sensor, get the inverted pendulum swing rod angle every sampling: ; After the inverted pendulum system is powered on, collect the swing rod angle data, take the average value in this group of data, and record this average value as the zero offset value ; According to the sampling frequency Collect the motor position , The value after eliminating the zero offset, the inverted pendulum swing rod angle , respectively through Fourier transform, get the frequency spectrum, find out the frequency of high frequency vibration ; Use the collection frequency And the frequency of high frequency vibration Make a pre low pass filter; Low pass filter: Through low pass filter, first filter out part of high frequency noise, get relatively smooth swing rod angle data And the position data of the swing rod carrier trolley , and according to the sampling frequency, calculate the swing rod angular velocity And the speed of the swing rod carrier trolley , , Low pass filter coefficient; When the inverted pendulum starts, collect the inverted pendulum swing rod angle and the inverted pendulum swing rod carrier trolley position according to the collection frequency After removing the zero offset, get , the motor position ; Test the minimum drive voltage of the motor, that is, the corresponding PWM value, and do amplitude control on the output voltage of the motor; According to And the motor position , judge the motion state of the inverted pendulum; Based on the current motion state, execute the motion control instruction: (1) when the inverted pendulum angle , , and < Is the set left stable angle threshold, Is the set left stable angle threshold, Is the left limit position of the trolley track, Is the right limit position of the trolley track, it is the swing state, at this time, output positive PWM for 1s, and then output negative PWM for 1s, realize the swing; (2) When the inverted pendulum angle , that is, the inverted pendulum pole angle is within ± 25°, at this time, it enters a stable state; The nonlinear dynamics model of the inverted pendulum including the motor model is established, and the Jacobian coefficient matrix is calculated, and the spatial state equation is established: The system noise is defined , the measurement noise , the measurement noise , the measurement noise , is the state transition matrix of the spatial state equation, is the output matrix of the spatial state equation, is the direct measurement matrix of the spatial state equation, is the output direct action matrix of the spatial state equation; Smooth data and are brought into the extended Kalman filter for composite filtering: is , where The constant optimal feedback gain vector can be iteratively calculated; (3) Combined with the state vector , the optimal gain is calculated using LQR and optimal control theory to adjust the pole to the stable state: Where is the terminal value, which is uncontrollable, and the minimum cost is calculated at , the input weight matrix is defined , the output weight matrix is defined , and the target state vector is defined ; The optimal control law is recursively obtained: Where is the optimal feedback gain, is the collected data after pre-low-pass filtering and extended Kalman filter composite filtering, and the corresponding voltage is output through the optimal control rate. The voltage is output in a linear proportion to control the inverted pendulum to stabilize the pole. (4) When the inverted pendulum car position ≥ or ≤ , at this time, in order to protect the car from colliding with the edge, it enters the protection state, at this time, no longer output PWM through the controller, directly output a fixed PWM value to promote the car to return to the origin.

[0014] When the inverted pendulum rod and the inverted pendulum rod carrier trolley deviate from the target position, the corresponding voltage value is output through the feedback gain, and the voltage value is converted into a PWM output in a linear proportion to control the motor; The determination of the filter coefficient of the pre-low-pass filter and its incorporation into the state estimation of the Kalman filter are one of the keys of the system. In the modeling of the inverted pendulum, the incorporation of the motor model output voltage value is another key point of the system. The third key point is the selection of the variances of the system noise and the measurement noise of the Kalman filter and the selection of the input weight and the output weight of the LQR algorithm. The closer the selected variances are to the true values, the more accurate the estimation of the collected data is, the better the system control effect is, the stronger the anti-interference ability is, the greater the output weight is, the slower the system response is, the weaker the anti-interference ability is, and the greater the emphasis of the input weight matrix on each state variable is.

[0015] Embodiment 2: The position and angle information acquisition module is used to acquire the inverted pendulum angle information and the inverted pendulum rod carrier trolley position information ; The pre-low-pass filter and the extended Kalman filter composite filtering module: The collected data are smoothed by a low-pass filter to reduce noise, and the obtained inverted pendulum angle information and trolley position information are obtained. The Kalman state estimation is: and are introduced into the state space equation linearized by the Jacobian matrix to obtain the prior state vector , The Kalman gain is and the state transition Jacobian matrix are multiplied, and Gaussian noise processing is performed to obtain the collected data with reduced Gaussian noise. The state judgment module: according to the current pendulum rod angle and the trolley position , the current motion state is judged. The calculation output module: According to the target state vector , is the optimal feedback gain calculated by the optimal control theory , the current output voltage value is calculated, and the voltage value is converted into a PWM value by the controller to output to the motor.

[0016] Embodiment 3: As Figure 1 , Figure 2As shown in the embodiment, the control device adopts a controller, and a control system of an inverted pendulum based on a composite of an EKF and an LPF, comprising: a synchronous pulley 1, a synchronous belt 2, an inverted pendulum carrier trolley 3, an angle potentiometer 4, a motor 5, an encoder 6, a pendulum rod 7, an inverted pendulum guide rail 8, and a controller. The angle potentiometer 4 is fixed on the inverted pendulum carrier trolley 3, and the shaft of the angle potentiometer is connected with the pendulum rod 7. The encoder 6 is installed behind the motor shaft 5, and is used to collect the rotation angle of the motor output. The synchronous belt 2 is connected with the synchronous pulley 1, and is used to transmit the power output by the motor, control the forward and backward movement of the inverted pendulum carrier trolley 3 on the inverted pendulum guide rail 8, and drive the movement of the pendulum rod 7. The PWM output port of the control device is connected with the drive board of the motor, and is used to control the forward and reverse rotation and the rotation speed of the motor. The controller comprises a memory and a processor, and the memory is used to store instructions for controlling the processor to operate to perform the control method of the inverted pendulum based on the composite of the EKF and the LPF described in Embodiment 1. In some embodiments, a data display module is connected with the controller to collect the angle data of the pendulum rod of the inverted pendulum, the position data of the motor encoder, and the filtered angle data of the pendulum rod of the inverted pendulum. The position data of the motor encoder is connected with the USB interface through the USART protocol and uploaded to the PC end to be marked out with curves of different colors for comparison of the filtering effect and adjustment of the filtering parameters. The controller adopts the STM32F103 chip of the STMicroelectronics Company, the angle potentiometer adopts the WDD35D-4 potentiometer, the encoder adopts the incremental Hall encoder, and the driving module adopts the TB6612 chip. The motor adopts a DC motor with a reduction ratio of 1:20 and is powered by a stabilized power supply to provide sufficient power for the motor.

[0017] The angle potentiometer has an angle jump, and when rotating at 360° and 0°, the detection data jump span is too large, which causes that the weak control of the pendulum rod in the stable state cannot be realized. The jump point is set at the direction of 120° of the pendulum rod.

[0018] Compared with the traditional low-pass filter, the influence of the process noise, Gaussian noise, and random noise is weakened in the filtering aspect, the data is closer to the true value, the control effect of the control strategy is effectively improved, the problems of unstable input data and large noise are solved, the advantages and disadvantages of different control strategies are facilitated to be studied, and the control system has the characteristics of low cost and simple device. The motor model is incorporated into the established mathematical model of the inverted pendulum, and the output voltage value is effectively improved, so that the control performance of the controller is improved, and the entire device has higher stability, robustness, and anti-interference performance.

[0019] The application is described with reference to flow diagrams of methods, systems, and computer program products in accordance with the embodiments. It is understood that each block of the flow diagrams, and combinations of blocks in the flow diagrams and blocks, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general purpose computer, special purpose computer, embedded processing machine, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flow diagrams Figure 1 one or more of the flow diagrams and / or blocks Figure 1 one or more of the flow diagrams and / or blocks

[0020] These computer program instructions can also be stored in a computer- readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instructions which implement the function specified in the flow diagrams Figure 1 one or more of the flow diagrams and / or blocks Figure 1 one or more of the flow diagrams and / or blocks

[0021] The above description is only preferred embodiments of the application. It should be pointed out that for those skilled in the art, some improvements and refinements can be made without departing from the principles of the application, and these improvements and refinements should also be considered as falling within the scope of the application.

Claims

1. A method, apparatus, and system for controlling an inverted pendulum based on a combination of EKF and LPF, characterized in that, include: The position of the vehicle is obtained by sampling using position sensors: ; Using an angle sensor, the angle of the inverted pendulum rod is obtained for each sample: ; Based on the current position of the inverted pendulum cart rocker arm angle Car speed Inverted pendulum angular velocity Preset car position Preset swing arm position An inverted pendulum-motor dynamic model was established. Through EKF and LPF composite filtering, iterative optimization was performed to stabilize the data acquisition. The output voltage was calculated using the LQR algorithm. Based on the calculated voltage, a PWM output is generated in a linear proportion, and commands are issued to control the motor speed.

2. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that, According to the sampling frequency Collected motor location Inverted pendulum rod angle By performing Fourier transforms on each component, the frequency spectrum is obtained, and the frequencies of the high-frequency vibrations are identified. ; Low-pass filter: For the first The vehicle's position data after passing through a low-pass filter For the first The angle data of the inverted pendulum rod after passing through a low-pass filter. These are the coefficients of a first-order low-pass filter. in It is the cutoff frequency. It is the sampling frequency.

3. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that, The data is obtained by filtering with a low-pass filter. , And set The time period is differentiated to obtain the velocity information. , ; Based on the current motion state, sample the state variable data of the inverted pendulum and combine them as follows: = 。 4. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that... ,include: A dynamic model was established, and the nonlinear state equations of the inverted pendulum were solved using observer theory and differential equations. Then, the data are fused and calculated using a low-pass filter embedded with an extended Kalman fusion filter algorithm, iteratively optimizing to obtain the state data closest to reality, reducing interference from random noise, Gaussian noise, and high-frequency noise. in It's about the car's position. Car speed , swing arm angle angular velocity of the pendulum Gaussian noise distribution It is a priori estimate. It is the posterior estimate.

5. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that, Using the principle of optimal control, the optimal feedback gain is calculated and solved: It is the output state vector. It is the input state vector combined with the cost function, let , The target state vector is used to iterate and obtain the optimal feedback gain. ,in It is the input weight matrix. It is the output weight matrix.

6. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that: Set the target state vector as ; By adjusting the dynamic model of the inverted pendulum system through feedback gain adjustment and incorporating a DC motor model, the output voltage is converted into a PWM output in a linear proportion to control the inverted pendulum to maintain a stable state.

7. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that, Based on angle values Determine the motion state of the inverted pendulum: When the inverted pendulum angle , ,and < It is the set left stability angle threshold. It is the set left stability angle threshold. This is the leftmost position of the trolley track. This is the right limit position of the trolley track. In the swing state, the swing is initiated by outputting a positive PWM for 1 second and then a negative PWM for 1 second. When the inverted pendulum angle At this point, it enters a stable state, and the PWM output is adjusted to a stable state through the optimal feedback gain. When the trolley is upside down, its position ≥ or ≤ At this point, in order to protect the car from colliding with the edge, it enters a protection state, and outputs PWM to make the car return to the origin.

8. The inverted pendulum control method based on the combination of EKF and LPF according to claim 1, characterized in that, include: The position and angle information acquisition module is used to collect the angle information of the inverted pendulum. and the position information of the inverted pendulum carrier trolley ; The combined low-pass and extended Kalman filter module passes the collected data through a low-pass filter to obtain the inverted pendulum angle information. and car location information and the inverted pendulum angle information and car location information Iterative optimization is performed by embedding the linearized state-space equation of the extended Kalman filter, reducing random errors and high-frequency noise, and obtaining the current state variables that are close to the true values. Status determination module: based on the current lever angle and car position Determine the current motion state; Calculation output module: based on the target state vector and feedback gain It outputs a voltage value and converts it into a PWM value.

Images