Hey all,
Maybe I'm tired and the answer is staring me in the face, but I thought I'd throw this problem out to see who can solve it.
In 2.7.5 I'm looking to add linear deceleration when approaching a waypoint. Right now we decelerate at a proportion of our distance. This is ok in certain situations, but it fails horribly when you're flying faster than 6m/s. Currently the copter will rapidly decelerate and then slow down too much as it approaches the WP. What we need is a continuous and linear deceleration of a set amount (say 100cm/s/s).
If you just do this by measuring time, you'll get burned by wind and aerodynamic effects. We need something that can look at the deceleration and adjust so we don't overshoot or stop short. A PID controller could work or something simpler. I also want to keep it computationally small and close approximations are fine.
We could run a mortgage style calc every GPS read as well.
Anyone know the best approach?
Jason
Tags:
To get things started I'll make my small contribution which are based on formulas pulled from this wikipedia page:
ideal linear acceleration = -2 * ( orig_velocity * desired_time_period + orig_distance ) / desired_time_period^2
(in the above formula you need to put either orig_velocity or orig_position as negative)
So if you're 10m from the target (i.e. orig_distance = 10)
and travelling towards it at 15m/s (i.e orig_velocity = -15)
and you want to stop on the target in 3 seconds (desired_time_period = 3)
your ideal constant deceleration over the 3 seconds would be 7.78m/s/s
Actually this may not be quite right..if I graph it it looks like it goes past zero and comes back..
Oh man, this stuff used to be my bread and butter, but it's been a long time.
One of the problems is that the equations include t, and frankly, we don't care about t. What we really need is to solve r0, for given inputs of r=0, v0=0 (we want to stop on target), a is some fixed number, let's say 5m/s^2 or roughly half a G of deceleration. And then a given input speed, like 10 m/s. What we want is to find the point r0 where we should start decelerating.
I'll work on it.
Jason,
For constant deceleration, the kinematics are simple. Depending on whether you want to know where to start decelerating, or how hard to decelerate, the equations are:
d = (V0*V0)/(2*a)
or
a = (V0*V0)/(2*d)
Where V0 is initial velocity in meters/second, a is deceleration rate in meters/(sec*sec), and d is distance to stopping point in meters.
That said, you probably do not want to do constant deceleration, you might want to "feather" the deceleration rate at the end, much like when you brake a car.
So, for example, what you might want to do is rotate at a certain rate until the tilt is the value that you want, decelerate for a while, and then slowly level as you come to the point.
At any rate, if you tell me the general shape of tilt versus time that you would like to use, it would be simple matter for me to work out the math for you to tell you when to start "braking" and what tilt profile to use.
Best regards,
Bill Premerlani
PS: here is the math for constant deceleration:
V = V0 - a*t, so t = V0/a
d = V0*t - 1/2*a*t*t
substitute t into the second equation and simplify
d= 1/2*(V0*V0)/a
Jason,
So, taking your deceleration rate of 1m/s/s, you could do this:
Compute deceleration distance = 1/2*(V0*V0), V0 in meters/second.
So, suppose you are going along at 10 meters/second. Then, stopping distance is 50 meters.
When you are within 50 meters, apply deceleration of 1 m/s/s.
You could also adjust the deceleration a bit as you decelerate according to:
a = 1/2*(V*V)/d, where d is distance to waypoint, V is instantaneous velocity.
When you get close to waypoint you would probably want to switch to some other method, to avoid division by zero.
For this method to work, you compute everything in earth frame, so if there is some wind, you will have to adjust tilt appropriately in order to get the desired deceleration in earth frame.
Best regards,
Bill
I think what we would want to do is fairly simple.
As we approach a waypoint where the copter is supposed to begin a loiter, we calculate the stop-short-point, which we now have the equation for. But what I would do is set a target speed of 1 m/s and a target distance of the waypoint radius.
Meaning, that was we hit the waypoint radius, we are going 1 m/2. At that point, maybe we switch over to the loiter controller.
Or, we what we just switch to a linear deceleration at that point. So at the WP radius, we are doing 1 m/s, and then ramp down to 0 m/s by the time we reach the target. I think this should get us a reasonably effective but crude "soft stop", without a ton of processing power.
Now one big remaining question is, can we use a similar process to make nice rounded corners when we are to fly a waypoint at speed? I might thing that what we could do is basically... say we are between WP 1 and 2. WP2 is due north of WP1. Then we have a WP 3 which is on a compass bearing 45° from WP2. Target WP speed is 10 m/s. So as we fly from WP1 to 2, our X target speed is 10 m/s, and Y target speed is 0. We also know that from WP2 to 3, the X target speed is 7.07 m/s, and the Y target speed is 7.07 m/s.
So we redo the calculation such that at some distance before WP 2, we start decelerating X speed from 10 m/s, to 7.07 m/s. Then we also accelerate the Y speed from 0 m/s to 7.07 m/s.
This might give us a reasonable approximation of the "Fly-By" WP turn discussed on the devlist.
Hi R_Lefebvre,
There is an elegant way to eliminate time from the math.
dV/dt = a
dx/dt = V
where dV/dt is time rate of change of velocity and dx/dt is time rate of change of position
Therefore, 1/2*d(V*V)/dx = a
Which is just another form of the energy equation.
So, for constant acceleration
1/2*(V2*V2 - V1*V1) = a*(x2-x1)
Best regards,
Bill
I did get the time out with the t=V/a, and then substituting that for 't' in the original equation. Actually surprised myself on that one. But I still struggled with some negatives because the initial conditions had some negatives and... well I got it eventually, just had to stare at it for a while.
Anyway, using your equation, I think it works. Very simple. Thanks!
So I think that all we need to do, is add some stuff into the Nav controller.
First thing, is to determine if this is a "stop at WP" or if it's a "fast WP" (which we already do).
If it's a stop at waypoint, set it up to start checking "should we be slowing down yet?" Then just do as I've suggested here, target 1 m/s by the time we hit WPrad.
If it's a fast waypoint, then we set the V2 target at, using my example above, 7.07 m/s, but at the actual waypoint.
The only hitch I see is that if we just do a continual check, if the velocity drops just a little below the target velocity, on the next check, it will think it needs to speed up. So we should allow that. Do these checks, ratchet down the velocity target, but don't let it increase.
Jason, do I understand this right, that we do fast_corner if the next WP is at less than 60° from the previous waypoint?
// Fast corner management
// ----------------------
if(tmp_index == -1) {
// there are no more commands left
}else{
// we have at least one more cmd left
Location tmp_loc = get_cmd_with_index(tmp_index);
if(tmp_loc.lat == 0) {
fast_corner = false;
}else{
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
temp = wrap_180(temp);
fast_corner = labs(temp) < 6000;
}
}
Done, anybody want to look?
https://code.google.com/r/robertlefebvre-wip2/source/list?name=Fast...
631 members
259 members
281 members
1470 members
3164 members
© 2019 Created by Chris Anderson. Powered by