Electric Competition 2015 Question B: Wind Pendulum Control System

The demo video has been uploaded to Station B

https://www.bilibili.com/video/BV11t4y1q72s

Question analysis

The wind pendulum control system is approximately regarded as a single pendulum control system

T=2Pi sqrt(L/g)

The length L of the thin rod we use is 787.3 mm. Assuming the acceleration of gravity g=9.8 m/s2 , the period of the simple pendulum is:

T=2Pi sqrt(0.787/9.8) = 1.7805 s

Since the angle is greater than 15°, the formula calculation is not accurate. Calculate ten cycles by counting the table, and the calculated cycle is 1596ms.

frequency

f=1/T=1/1.596=0.6265 Hz

It can be seen that we need to design a control system with a bandwidth greater than 0.6265 Hz. According to the Nyquist sampling theorem, the angle sampling rate can theoretically be fs>2f , but the question requires the maximum adjustment time of the system. In order to make the control effect better, fs>10f or even higher needs to be set. In this design In this example, we set the sampling rate of 20602 to 1K, and turned on the internal DLPF filter. The cut-off frequency is 5Hz. The control period T=1ms.

System Block Diagram

The wind pendulum control system uses STM32F072 as the main control, and the onboard ICM20602 attitude sensor can be optionally attached to 6050 for wind pendulum motion attitude detection.

At the same time, the wind pendulum uses a 716 motor as the motion execution system.

Output four hardware PWMs through STM32 and drive four coreless motors through MOS tubes.

The measured current of the coreless motor can reach 1.5A, and a high-power MOS is required.

Selection instructions

1. STM32F072 is a low-power Cortex-M0 processor launched by ST, with a 48MHz main frequency and 16K SRAM. Sufficient for wind pendulum applications.
2. This question does not have high performance requirements. I happened to be looking at the F0 series of processors at the time, so I thought about using F0 to give it a try.
3. The IMU uses ICM-20602, which is also a commonly used IMU. MPU-6050 can also achieve it, and there is not much difference in performance between the two. Maybe 20602 has slightly better performance, but we just want to verify the chip used in ICM-20602.
4. The overall structure uses PCB as the main structural component, and a carbon fiber tube (to increase rigidity) fixes the wind pendulum itself to the hanging wooden bars through flange couplings at both ends. The IMU is installed on the PCB and swings along with the entire system. The attitude of the system is determined through the acceleration and angular velocity output by the IMU, thereby achieving closed-loop feedback control of the system's state. Four coreless motors are installed symmetrically along the diagonal line of the PCB to obtain better mechanical symmetry.

Principle description

1. The first solution we tried was to use an algorithm similar to flight control: after collecting the gyroscope data, we used quaternions to solve the pendulum posture, then back-solved the Euler angle, and used a PID controller to adjust the Euler angle to follow a certain line. Desired swing curve. However, the data output by the accelerometer during the swing is meaningless and cannot participate in attitude calculation. Therefore, the attitude calculated purely using angular velocity will drift over time. After a period of time, the resting position of the backswing may even deviate significantly from the plumb line due to the zero bias of the posture data. At the same time, due to the flaws of this attitude calculation algorithm itself, after realizing the "drawing a circle" requirement of the question, the calculated direction angle will drift significantly, making it impossible to complete the requirement of swinging a straight line with a fixed angle. Eventually we gave up this form of control.
2. We took a look at other people's implementation plans. Many plans say that the core is PID, but what bothers us is the solution of this attitude. We believe that the attitude solution is the core of this problem. Many solutions are Directly use the 6050's DMP to read the posture, but there is no DMP library on the 20602....
3. After we analyzed the wind pendulum system, we believed that the angle curve of the wind pendulum is difficult to follow using a PID controller due to attitude calculation problems. We can also achieve this by following the angular velocity curve or angular acceleration during swing. The feedback data that the controller needs when following the angular velocity is the real-time angular velocity data directly from the gyroscope, which avoids the integration error caused by re-integrating the angular velocity, can achieve better stability, and is less likely to cause the overall system to drift. When following angular acceleration, the output is torque, which can be directly controlled by the motor and has natural control advantages. However, because the hollow core motor we used cannot control the force output by the motor very well, we finally adopted a solution that follows the angular velocity curve. In fact, if the fan is very powerful, it is completely possible to control the pendulum to point at a certain angle. Experimental results prove that the control performance of this solution is also very good. Our core control algorithm is these two lines. The control formula is as shown in the figure:

Pitchand Roware the angles in the x-direction and y-direction respectively. The corresponding Omegais the angular velocity of the corresponding angle.