Analysis of four-axis fixed height control based on ultrasonic wave

The author is from Wuhan Science and Technology of Xiao Qing classmate, next for everyone to talk about based on the high-quality ultrasound, due to the author's limited ability, so if there are errors please advice, and a lot of just engineering approximation application, has not done simulation simulation.

What I'm talking about is more practice-oriented and can help you to realize this application effectively!

Based on the high-precision ultrasonic, there are several problems:

Hardware and Installation tips?

Ultrasonic ranging algorithm (how to program, how to deal with the results)?

What kind of pid algorithm is used?

How to debug more quickly?

What are some of the tough issues you will encounter in real development?

How does the fixed height control achieve more stability for takeoff and landing?

The author made a high use of one weeks, and reference to other algorithms, the implementation is relatively simple, but really achieve a good effect, but experienced a lot of setbacks.

So I'm going to start the discussion:

1 is my four-axis platform, spent about 2000,PCB is to do, the module is their own, debugging ground station is anonymous ground station, through the Bluetooth communication; Of course, this is not the theme of today, I assume you have got a good attitude control.

I. hardware and Installation tips:

1. The characteristic of ultrasound is that, when the module is suddenly obscured by something (1cm or less), then the measured value will be mutated, very large, then this is destructive to the control. So your ultrasound should be installed just below the aircraft, and avoid being put together with the batteries. Easy to interfere. In addition, threshold processing is required, and if the measured value is mutated, the measurement results are discarded.

In theory, the ultrasound is placed in the center of the aircraft is the best, so that the distance is more accurate measurement. 2. If the pitch angle of the aircraft is not 0, it is mathematically compensated, but in fact you can calculate that the distance from the angle changes within 10 is very small. So do not need the ppt inside the geometry compensation, and in the STM32 trigonometric function calculation is very large, even if it is replaced by fast trigonometric functions, resulting in the calculation error, far greater than the actual measurement error, compensation instead of the outweigh the value. And there is no obvious shift in ultrasound. The authors of PPT may not have actually implemented them. The ultrasonic wave does not need to be mounted directly underneath, it can be installed on both sides of the bracket. Because it's just underneath the camera.

If the program is not sophisticated design, unreasonable threading design, unreasonable calculation design, will cause the program out of control.

2. How to measure and process data:

Ultrasonic some use the serial port protocol, I personally think the serial protocol is good use, the following is the program, statement 1 means that the measured data is two bytes obtained, the first byte shifted left eight bits, plus the second byte. The distance, unit is mm, the author measured, ultrasonic module precision is very high.

The measurement gets the original ultra_distance, needs to be processed, otherwise the obtained data is up and down jitter.

I did not save the diagram, so the following is not very obvious, or can see the beat. There are typically 3 filtering algorithms:

1. Mean Value Filter

2. Low-pass filtering

3.kalman filtering

The author measured, the 23rd effect is similar, and the low-pass filter is more easy to achieve, the first kind of effect is very poor, then we look at low-pass filtering.

Here for different heights to do a different frequency of low-pass filtering, this form is based on the frequency of filtering, relatively difficult to understand, we can look at the following:

yn=a* xn+ (1-a)*yn-1

Yn-1 is the last measured value,Yn is the measured value, the output, the cutoff frequency is

F = A/(2*3.14*t)

T is the sampling period;

As above is the code implementation, which by the right to move 8 bits to achieve division, faster. Calculate the available cutoff frequency is

Second, what kind of PID is used ?

Of course, the Cascade PID.

First -order PID is also possible. But obviously unstable, and control complex. Different parameters need to be adjusted at different heights. Cascade PID is more stable and less problematic. Is the PID algorithm for Pix Hawk . In fact, I use the inner ring is a speed pid, that is, the speed of the aircraft rise and fall, the outer ring is of course a high pid.

The whole realization does not speak, is the PID algorithm, says the detail. How to get speed, for example, requires a fusion filtering algorithm:

y = (a) * (y) + (1-a) * (x);

This is different from the low-pass filter is a first-order fusion filter, the two measurement results, combined, taking into account two good results, offset the negative effect.

Wz-speed is the velocity measured by the acceleration of gravity, and theh_speed is obtained by ultrasonic differential. One sentence of the code is the integration of the acceleration, the second sentence is dead zone control, a bit troublesome, not mentioned here.

Three or four sentence to the acceleration measurement of low-pass filtering, the fifth sentence, through the fusion of two speeds, to obtain more stable results. You can look at this curve on the ground station, it is very beautiful, but the speed of the reply a little bit slow.

The outer ring is the height of the control, there are several points to note:

1. The height must be limited, can not be too high;

2. When the drone starts to take off from the ground, to set a fixed large height value, provide a stable pid control, more than a certain height and then change the target height, for example, when the UAV is still measured 20CM, then open the throttle set target height of 40, The plane rose to 40 and continued to add altitude. Because the drone has a ground effect, there is greater thrust on the ground.

Here is the experience gained from engineering practice.

Third, how to debug more quickly?

What are some of the tough issues you will encounter in real development?

How to debug the high, really very laborious. You need to change the PID repeatedly.

And there are a lot of difficult problems, I was debugging, because I came back a head, the results did not see the drone flew to the world, the ceiling crashed. The four-axis bottom is hard to tune, often problematic. It would be devastating to make a problem worse.

I've added two pillars in two directions on the four axes, so when it's not good to fly, you can grab it. There is a second advantage, that is, the height of the hover is not, so the plane floating around, it is difficult to adjust the height, you can not poke a rod, let him keep in a range. There is also a benefit to measure the stability of your posture. Good stability, you go to the violent swing pole, posture can quickly recover. Will not crash. But do not use a pole to rotate the four-axis, so basic avalanche.

First adjust the inner ring. Outer ring no matter. Add the throttle to allow the drone to take off. Next, the p, the p is transferred to a suitable position, you violently up and down the four-axis, no chance of a large damping, the more smooth damping, the faster the better, you will four-axis upward push, four-axis speed down, push down the speed of the start to see your thrust. Tune better after the bare-handed open four-axis, you will find the four-axis slowly falling, if the speed of 0 to maintain the better performance, the better the p. And then D,D, almost on the line. The key is I, which takes a long time to debug to feel. Without I, the effect is very poor.

Then is to adjust the height of the ring, first adjust the high, then adjust 100cm fixed high, then 30cm, the lower the more difficult to adjust. Four-axis stable to reach the destination that's the best,

Here's a tip:

The exp_height is a fixed height control value. Height_value is the real target value, not overnight, so the expectations of the high, to slowly approaching him. Otherwise give a big value, it is easy to overshoot, a little closer to the target, will be more stable. This time only need to slowly adjust the approximate speed.

Four, how to achieve a stable landing?

This is the most engineering, and the programming difficulty is actually very large.

In terms of landing, first lower than a threshold must be turned off the motor, or around the ground will jump, unstable;

Then falling from the high, better way is open-loop, slowly lower throttle, you can slowly fall down. Higher than the method of control height;

Takeoff is also very rare. Today first will be here, next time to talk about, stable landing.

Analysis of four-axis fixed height control based on ultrasonic wave