Context
My task is to design and build a velocity PID controller for a micro quadcopter than flies indoors. The room where the quadcopter is flying is equipped with a camera-based high accuracy indoor tracking system that can provide both velocity and position data for the quadcopter. The resulting system should be able to accept a target velocity for each axis (x, y, z) and drive the quadcopter with that velocity.
The control inputs for the quadcopter are roll/pitch/yaw angles and thrust percentage for altitude.
My idea is to implement a PID controller for each axis where the SP is the desired velocity in that direction, the measured value is the velocity provided by the tracking system and the output value is the roll/pitch/yaw angle and respectively thrust percent.
Unfortunately, because this is my first contact with control theory, I am not sure if I am heading in the right direction.
Questions
I understand the basic PID controller principle, but it is still
unclear to me how it can convert velocity (m/s) into roll/pitch/yaw
(radians) by only summing errors and multiplying with a constant?
Yes, velocity and roll/pitch are directly proportional, so does it
mean that by multiplying with the right constant yields a correct
result?
For the case of the vertical velocity controller, if the velocity is set to 0, the quadcopter should actually just maintain its altitude without ascending or descending. How can this be integrated with the PID controller so that the thrust values is not 0 (should keep hovering, not fall) when the error is actually 0? Should I add a constant term to the output?
Once the system has been implemented, what would be a good approach
on tuning the PID gain parameters? Manual trial and error?
The next step in the development of the system is an additional layer
of position PID controllers that take as a setpoint a desired
position (x,y,z), the measured positions are provided by the indoor
tracking system and the outputs are x/y/z velocities. Is this a good approach?
The reason for the separation of these PID control layers is that the project is part of a larger framework that promotes re-usability. Would it be better to just use a single layer of PID controllers that directly take position coordinates as setpoints and output roll/pitch/yaw/thrust values?
That's the basic idea. You could theoretically wind up with several constants in the system to convert from one unit to another. In your case, your P,I, D constants will implicitly include the right conversion factors. for example, if in an idealized system you would want to control the angle by feedback with P=0.5 in degrees, bu all you have access to is speed, and you know that 5 degrees of tilt gives 1 m/s, then you would program the P controller to multiply the measured speed by 0.1 and the result would be the control input for the angle.
I think the best way to do that is to implement a SISO PID controller not on height (h) but on vertical speed (dh/dt) with thrust percentage as the ouput, but I am not sure about that.
Unfortunately, if you have no advance knowledge about the parameters, that will be the only way to go. I recommend an environment that won't do much damage if the quadcopter crashes...
That sounds like a good way to go
My blog may be of interest to you: http://oskit.se/quadcopter-control-and-how-to-really-implement-it/
The basic principle is easy to understand. Tuning is much harder. It took me the most time to actually figure out what to feed into the pid controller and small things like the + and - signs on variables. But eventually I solved these problems.
The best way to test PID controller is to have some way to set your values dynamically so that you don't have to recompile your firmware every time. I use mavlink in my quadcopter firmware that I work on. With mavlink the copter can easily be configured using any ground control software that supports mavlink protocol.
Start small, build a working copter with using a ready made controller, then write your own. Then test it and tune it and experiment. You will never truly understand PID controllers unless you try building a copter yourself. My bare metal software development library can assist you with making it easier to write your software without having to worry about hardware driver implementation. Link: https://github.com/mkschreder/martink
If you are using python programming language, you can use ddcontrol framework.
You can estimate a transfer function by SISO data. You can use tfest function for this purpose. And then you can optimize a PID Controller by using pidopt function.
Related
I am new in python or any programming language for that matter. For months now I have been working on stabilising the inverted pendulum. I have gotten everything working but struggling to get the right reward function. So far, after researching and trials and fails, the best I could come up with is
R=(x_dot**2)+0.001*(x**2)+0.1*(theta**2)
But I don't get to stability, this being theta=0 long enough.
Does anyone has an idea of the logic behind the ideal reward function?
Thank you.
For just the balancing problem (not the swing-up), even a binary reward is enough. Something like
Always 0, then -1 when the pole falls. Or,
Always 1, then 0 when the pole falls.
Which one to use depends on the algorithm used, the discount factor and the episode horizon. Anyway, the task is easy and both will do their job.
For the swing-up task (harder than just balancing, as the pole starts upside down and you need to swing it up by moving the cart) it is better to have a reward depending on the state. Usually, the simple cos(theta) is fine. You can also add a penalty for the angle velocity and for the action, in order to prefer slow-changing smooth trajectory.
You can also add a penalty if the cart goes out of the boundaries of the x coordinate.
A cost including all these terms would look like this
reward = cos(theta) - 0.001*theta_d.^2 - 0.0001*action.^2 - 100*out_of_bound(x)
I am working on inverted pendulum too.
I found the following reward function which I am trying.
costs = angle_normalise((th)**2 +.1*thdot**2 + .001*(action**2))
# normalize between -pi and pi
reward=-costs
but still have a problem in choosing the actions, maybe we can discuss.
I am a master student working with localization in VANEts
in this moment I am working on a trilateration method based on RSSI for
Cooperative Positioning (CP).
I am considering the Analogue Model : Simple Path Loss Model
But I have some doubts in how to calculate the distance correctly for a determined Phy Model.
I spent some time (one day) reading some papers of Dr. Sommer about the PHY models included in veins.
Would anyone help-me with this solution?
I need a way to:
1) Measure the power of an receiver when its receive a beacon (I found this in the Decider class).
In the Decider802.11p the received Power can be obtained with this line in method Decider80211p::processSignalEnd(AirFrame* msg):
double recvPower_dBm = 10*log10(signal.getReceivingPower()->getValue(start));
2) Apply a formula of RSSI accordingly the phy model in order to achieve a distance estimation between transmiter and receiver.
3) Asssociate this measure (distance by RSSI) with the Wave Short Message to be delivered in AppLayer of the receiver (that is measuring the RSSI).
After read the paper "On the Applicability of Two-Ray Path Loss Models for Vehicular Network Simulation"
and the paper "A Computationally Inexpensive Empirical Model of IEEE 802.11p Radio Shadowing in Urban Environments"
and investigating how it works in the veins project. I noticed that each analogue model have your own path loss model
with your own variables to describe the model.
For example for the SimplePathLossModel we have these
variables defined on AnalogueModels folder of veins modules:
lambda = 0.051 m (wave length to IEEE 802.11p CCH center frequency of 5.890 GHz)
A constant alpha = 2 (default value used)
a distance factor given by pow(sqrDistance, -pathLossAlphaHalf) / (16.0 * M_PI * M_PI);
I found one formula for indoor environments in this link, but I am in doubt if it is applicable for vehicular environments.
Any clarification is welcome. Thanks a lot.
Technically, you are correct. Indeed, you could generate a simple look-up table: have one vehicle drive past another one, record distance and RSSIs, and you have a table that can map RSSI to mean distance (without knowing how the TX power, antenna gains, path loss model, fading models, etc, are configured).
In the simplest case, if you assume that antennas are omnidirectional, that path loss follows the Friis transmission equation, that no shadow fading occurs, and that fast fading is negligible, your table will be perfect.
In a more complicated case, where your simulation also includes probabilistic fast fading (say, a Nakagami model), shadow fading due to radio obstacles (buildings), etc. your table will still be roughly correct, but less so.
It is important to consider a real-life application, though. Consider if your algorithm still works if conditions change (more reflective road surface changing reflection parameters, buildings blocking more or less power, antennas with non-ideal or even unknown gain characteristics, etc).
Impulse response is usually used in filter and for convolution but i always find it difficult to explain my self what is this and how does it help.
My question what is practical meaning of impulse response, either it an equation or characteristic of a system in response to input.
Definition:
In signal processing, the impulse response, or impulse response function, of a dynamic system is its output when presented with a
brief input signal, called an impulse.
Explanation: Think of it from a system perspective. Lets look at an airplane.
Sitting at the controls, a test pilot will get to a smooth level flight. Then they will push the flight stick all the way forward, then bring it all the way back to center position, as fast as possible.
They then see what the planes response is. Some will just drop in altitude, then level off. Some will start to dive and keep diving at a constant rate, and some (OH NO!) will start to dive faster and faster!
So knowing what the impulse response is, goes a long way in telling the characteristics of a system.
A designer should know what the impulse response should be, then design the system to it.
Resource:
http://en.wikipedia.org
http://www.edaboard.com/thread129957.html
The definition of impulse is given by the function
δ(t)= { 1 , t=0 | 0 , otherwise }
When you want to analyze a system from its frequency, you transform the time domain to the frequency.
First we find the transfer function of the system, so we spent this function to the frequency domain (Fourier transform).
The system input and output are related by Y(jW)=H(jW)*X(jW)
Where Y(jW) is the output, X(jW) is the input and H(jW) is our transfer function.
To analyze how our system behaves in frequency, we take as input X(jW) a unit impulse.
Applying the Fourier transform for δ(t) we have δ(jW)=1
Y(jW)=H(jW)*1 ---> Y(jW)=H(jW)
Thus our output does not change with the entry of a unit impulse, and we can analize our system in two different domains.
This is usually used for filter projects. However, there are several other applications for this tool.
Specifically, I am talking about programming for this contest: http://www.nodewar.com/about
The contest involves you facing other teams with a swarm of spaceships in a two dimensional world. The ships are constrained by a boundary (if they exit they die), and they must continually avoid moons (who pull the ships in with their gravity). The goal is to kill the opposing queen.
I have attempted to program a few, relatively basic techniques, but I feel as though I am missing something fundamental.
For example, I implemented some of the boids behaviours (http://www.red3d.com/cwr/boids/), but they seemed to lack a... goal, so to speak.
Are there any common techniques (or, preferably, combinations of techniques) for this sort of game?
EDIT
I would just like to open this up again with a bounty since I feel like I'm still missing critical pieces of information. The following is my NodeWar code:
boundary_field = (o, position) ->
distance = (o.game.moon_field - o.lib.vec.len(o.lib.vec.diff(position, o.game.center)))
return distance
moon_field = (o, position) ->
return o.lib.vec.len(o.lib.vec.diff(position, o.moons[0].pos))
ai.step = (o) ->
torque = 0;
thrust = 0;
label = null;
fields = [boundary_field, moon_field]
# Total the potential fields and determine a target.
target = [0, 0]
score = -1000000
step = 1
square = 1
for x in [(-square + o.me.pos[0])..(square + o.me.pos[0])] by step
for y in [(-square + o.me.pos[1])..(square + o.me.pos[1])] by step
position = [x, y]
continue if o.lib.vec.len(position) > o.game.moon_field
value = (fields.map (f) -> f(o, position)).reduce (t, s) -> t + s
target = position if value > score
score = value if value > score
label = target
{ torque, thrust } = o.lib.targeting.simpleTarget(o.me, target)
return { torque, thrust, label }
I may have implemented potential fields incorrectly, however, since all the examples I could find are about discrete movements (whereas NodeWar is continuous and not exact).
The main problem being my A.I. never stays within the game area for more than 10 seconds without flying off-screen or crashing into a moon.
You can easily make the boids algorithm play the game of Nodewar, by just adding additional steering behaviours to your boids and/or modifying the default ones. For example, you would add a steering behaviour to avoid the moon, and a steering behaviour for enemy ships (repulsion or attraction, depending on the position between yours and the enemy ship). The weights of the attraction/repulsion forces should then be tweaked (possibly by genetic algorithms, playing different configurations against each other).
I believe this approach would give you already a relatively strong baseline player, to which you can start adding collaborative strategies etc.
You can achieve the best possible flocking behaviour using control theory. We can start by expressing the state of a ship (a vector of positions and velocities) as variable X. We will use x to represent a small deviation from some state X0. For each node, linearize around the state you want the individual ships to be at:
d/dt(X) = f(X - X0)
where X0 is the state you want the ship to be at. f() can be nonlinear (as in the case of your potential field). Close to this point, the will obey
d/dt(x) = Ax
A is a fixed matrix which you should tweak to achieve stability. Much of the field of control theory tells you how to do this. It's very important to you that A leads to a stable system and that it converges fast. You should now break out MATLAB or GNU Octave. You should also read up on what poles mean in control theory, and Laplace transforms. The poles of the system (eigenvalues of matrix A) will characterize the response of the ship to disturbances of any kind, its stability, and its convergeance speed. It will also tell you whether the ship will move towards X0 or oscillate around X0.
There are several ways of choosing A (You probably don't get full control over A because of the game's limited physics) without having to do much analysis yourself. Two such algorithms are optimal control (You define the cost of controlling the system vs the cost of deviating from X0), and pole placement (You choose where the poles will go).
The state space theory also applies to an entire flock of ships, deviating from the configuration desired by every ship at that point in time. If the system is nonlinear, You might not be able to guarantee that all configurations are stable.
Conclusion:
Use control theory to guarantee individual stability, calculating X0 based on the ships around you, and add a potential field to prevent ships from bumping into eachother and moons.
Disclaimer:
There are several ways of expressing a state space system. This is the simplest but you'll need to express it differently when considering the open loop system (split up A into another 'A' which gives you the physical system and some other matrices which give you the control parameters)
Does anyone know of anywhere I can find actual code examples of Software Phase Locked Loops (SPLLs) ?
I need an SPLL that can track a PSK modulated signal that is somewhere between 1.1 KHz and 1.3 KHz. A Google search brings up plenty of academic papers and patents but nothing usable. Even a trip to the University library that contains a shelf full of books on hardware PLL's there was only a single chapter in one book on SPLLs and that was more theoretical than practical.
Thanks for your time.
Ian
I suppose this is probably too late to help you (what did you end up doing?) but it may help the next guy.
Here's a golfed example of a software phase-locked loop I just wrote in one line of C, which will sing along with you:
main(a,b){for(;;)a+=((b+=16+a/1024)&256?1:-1)*getchar()-a/512,putchar(b);}
I present this tiny golfed version first in order to convince you that software phase-locked loops are actually fairly simple, as software goes, although they can be tricky.
If you feed it 8-bit linear samples on stdin, it will produce 8-bit samples of a sawtooth wave attempting to track one octave higher on stdout. At 8000 samples per second, it tracks frequencies in the neighborhood of 250Hz, just above B below middle C. On Linux you can do this by typing arecord | ./pll | aplay. The low 9 bits of b are the oscillator (what might be a VCO in a hardware implementation), which generates a square wave (the 1 or -1) which gets multiplied by the input waveform (getchar()) to produce the output of the phase detector. That output is then low-pass filtered into a to produce the smoothed phase error signal which is used to adjust the oscillation frequency of b to push a toward 0. The natural frequency of the square wave, when a == 0, is for b to increment by 16 every sample, which increments it by 512 (a full cycle) every 32 samples. 32 samples at 8000 samples per second are 1/250 of a second, which is why the natural frequency is 250Hz.
Then putchar() takes the low 8 bits of b, which make up a sawtooth wave at 500Hz or so, and spews them out as the output audio stream.
There are several things missing from this simple example:
It has no good way to detect lock. If you have silence, noise, or a strong pure 250Hz input tone, a will be roughly zero and b will be oscillating at its default frequency. Depending on your application, you might want to know whether you've found a signal or not! Camenzind's suggestion in chapter 12 of Designing Analog Chips is to feed a second "phase detector" 90° out of phase from the real phase detector; its smoothed output gives you the amplitude of the signal you've theoretically locked onto.
The natural frequency of the oscillator is fixed and does not sweep. The capture range of a PLL, the interval of frequencies within which it will notice an oscillation if it's not currently locked onto one, is pretty narrow; its lock range, over which it will will range in order to follow the signal once it's locked on, is much larger. Because of this, it's common to sweep the PLL's frequency all over the range where you expect to find a signal until you get a lock, and then stop sweeping.
The golfed version above is reduced from a much more readable example of a software phase-locked loop in C that I wrote today, which does do lock detection but does not sweep. It needs about 100 CPU cycles per input sample per PLL on the Atom CPU in my netbook.
I think that if I were in your situation, I would do the following (aside from obvious things like looking for someone who knows more about signal processing than I do, and generating test data). I probably wouldn't filter and downconvert the signal in a front end, since it's at such a low frequency already. Downconverting to a 200Hz-400Hz band hardly seems necessary. I suspect that PSK will bring up some new problems, since if the signal suddenly shifts phase by 90° or more, you lose the phase lock; but I suspect those problems will be easy to resolve, and it's hardly untrodden territory.
This is an interactive design package
for designing digital (i.e. software)
phase locked loops (PLLs). Fill in the
form and press the ``Submit'' button,
and a PLL will be designed for you.
Interactive Digital Phase Locked Loop Design
This will get you started, but you really need to understand the fundamentals of PLL design well enough to build it yourself in order to troubleshoot it later - This is the realm of digital signal processing, and while not black magic it will certainly give you a run for your money during debugging.
-Adam
Have Matlab with Simulink? There are PLL demo files available at Matlab Central here. Matlab's code generation capabilities might get you from there to a PLL written in C.