Copter: reduce cutting corners during missions

[andreas-eberle]

Issue details

My Copter didn't follow the autopilot route completely. At some corners, it took a shortcut causing it to not reach several waypoints at all.

After the flight I downloaded the logfile from my hexacopter. When viewing the generated .kmz file in Google Earth, you can easily see the shortcuts the copter took instead of following the waypoints.

The waypoints were created with the SimpleGrid AutoGrid mode with a line spacing of 10 meters, an inline waypoint distance of 10 meters and a waypoint radius of 2 meters. The waypoints are spline waypoints. Furthermore, I added camera trigger commands after every waypoint (I know that there is the automatic camera triggering after x meters). However, this shouldn't effect the waypoint following of the copter.

Version

Arducopter 3.5.1 and MissionPlanner 1.3.49

Platform

[ ] All [ ] AntennaTracker [x] Copter [ ] Plane [ ] Rover

Airframe type

Hexacopter

Hardware type

Pixhawk 2.1

Logs

logs.zip

[andreas-eberle]

I did another test flight with normal waypoints. Furthermore, I only used waypoints ad the border of the polygon (I created the waypoints with the SimpleGrid again).

As you can see in the screenshot from GoogleEarth, the copter did again shortcut the waypoints at the end of the lanes. However, the lanes have a spacing of 10 meters and I even set the waypoint radius to 1 meter. So it definetly did never reach the three edges in the screenshot.

Here are the logs and the waypoints: logs.zip

What I also noticed: At around the location where the turn stats to shortcut the end position of the lane, the autopilot made its "waypoint reached" beep. Is it thinking it already reached the waypoint? I mean it is clearly way more than 1 meter away from it.

[iskess]

Please see my open case #6194.

[rmackay9]

This is the expected behaviour actually so I've added the enhancement label. A couple of things will help reduce the short-cutting:

• add a 1 second delay onto the waypoint parameter (type "1" into the first column of the WAYPOINT command)
• increase the WPNAV_ACCEL parameter from 100 to 250 to shorten the "leash".
• reduce the WPNAV_SPEED to shorten the leash

[andreas-eberle]

Thanks for your answer.

In a later flight I actually increased WPNAV_ACCEL to 300 but also WPNAV_SPEED to 1500. Obviously there was still a rather large shortcut.

Is it possible to add a delay of like 0.001 seconds? Because I need to fly a route as long as possible and not to wait at every single corner for a second.

But my real question is: How can this be an expected behaviour? I mean, when I draw a route for the auto pilot, I expect it to follow this route and not to shortcut it by flying through an area I would never expect it to fly through (and which I cannot know beforehand). What if my waypoints go around a tree or building? The copter would fly straight into it. And how far away should I place my waypoints around that tree if I cannot see the shortcut in the editor (and don't want to use delay)? What is the reason for not using the waypoint radius like in plane?

[mboland]

I'm a little gobsmacked at the comment @rmackay9 that 'This is the expected behaviour actually'. If the waypoint radius is set to a value then I would have expected the copter (as I have actually seen in the past) to fight it's way into that waypoint before continuing. Are you now saying that waypoints are arbitrary?? I really don't understand the comment. Is this a new behaviour for auto missions? Sorry for butting in but I am really puzzled and concerned. I trust I have just misconstrued the comment.

[sashan13]

Spline waypoints aren't waypoints in traditional sense, they are spline vortexes. Read up about splines on Wikipedia. You should put waypoints further to compensate for that.

[andreas-eberle]

@sashan13: When you read the formulas for how to compute a cubic spline on the wikipedia page, you will see that a standard spline always goes through the points you specify as input. This is one of the great features of splines. The second feature is that you can achieve continuity at the given points making the curve smooth. Also, Mission Planner shows the spline path as one would expect it but the autopilot doesn't fly such a path.

Why do you think spline waypoints should not require the copter to fly through the actual waypoints?

[amilcarlucas]

@andreas-eberle please just do what @rmackay9 asked you to, and please reduce WP_NAVSPEED instead of increasing it. Use normal non-spline waypoints. and post the result here... I think you will all see that it improves a lot.

[andreas-eberle]

@amilcarlucas: Yes, with a lower WP_NAVSPEED (which I had before I increased it and then saw the drone doing shortcuts), it was not doing such extreme shortcuts. Also, not using spline waypoints helps a bit, but as you can see in the second screenshot I posted at the begining here, it is still a problemm.

Furthermore I want to use the drone to do work with it. So I need the 10 m/s I'm flying with. THe drone can easily do 15m/s and so it's not an option to spend 2 or 3 times the amount of time just because the software has a crazy behavior.

I understand that it might be some work to fix this, but using such a workaround is not a fix. If you are shown a route by Mission Planner that makes total sense to you, the drone must not do something completely different which you would never even guess or can estimate. And again, I cannot fly some arbitrary route when I want to achieve a certain goal with the drone (e.g. take pictures in a regular grid of a large area).

So please, fix this and make waypoints as well as spline waypoints work the way everybody expects it. I mean, it is working if you use waypoint delays. But again, I don't want to have the drone break down to 0 and sit at every waypoint for a second. I just want that behavior without the waiting time.

[edwinhayes]

@rmackay9 and other devs: is there any work underway, or planned intention, to address this issue in the foreseeable future?

I understand why this is expected behaviour based on the way the nav controller works at present, but it doesn't really seem that desirable: as raised above, it seems like a reasonable user could plot waypoints which travel around an obstacle and be unaware that the aircraft may not even try to hit some of them.

I thought I recalled a thread about adding a parameter or other method of selecting different waypoint hit-box behaviours, but I now can't recall where.

[rmackay9]

@edwinhayes, it's on @lthall's to-do list some time after we get through "new-loiter" I think. A short-term work around is to add a delay on each parameter. That will make the vehicle stop at the waypoint. It's not a perfect solution of course.

[lthall]

Hi all, This is the expected behaviour if you don't stop at each way-point. The intersection of any two lines that require a change in direction result in an infinite acceleration at the intersection if the speed is not reduced to zero then increased again.

So we provide two options. The first is the one you see here where the aircraft attempts to maintain the desired speed and the size of the corner is limited by the acceleration the aircraft can achieve. So slower speeds results in tighter corners and higher accelerations result in tighter corners. So this is the "get there as fast and as efficiently as possible" mode.

The second option is the "follow the path as close as we tell you" option. Here you put a small delay at each way-point. In this case the aircraft will slow as it approaches the way-point and then as soon as it enters the way-point radius it will count down the delay and continue onto the next way-point. Between the delay and the way-point radius you can get pretty close to any desired behaviour.

We are in the first steps of upgrading the navigation controller to enable some more advanced capability. We will then be able to have the aircraft:

• maintain maximum speed and cut the corner at a radius based on it's acceleration
• following a corner radius that meets the way-point radius at a tangent
• or comes to a complete stop at the way-point for a predefined time.

This is all extremely fiddly and time consuming but we will slowly get there. There is always a chance that some big company needs it done quickly and can afford to pay for our time but 99% of this work is done for no other reason than we want to make the code as good as we can.

[rmackay9]

I just want to point out that the last item, "or comes to a complete stop at the way-point for a predefined time" is already done. Just set the first field (labelled "Delay") of the waypoint mission command to a number which represents the number of seconds the vehicle should stop for.

[edwinhayes]

Are there not two valid interpretations of the first operating mode, where the cornering is limited by the aircraft's acceleration capability? One is that the aircraft maintains its speed, undershoots the corner, in which case it might not enter the waypoint radius, but guarantees that it won't hit obstacles on the outside of the planned flightpath; the other is that it maintains its speed, overshoots, passes through the waypoint radius, and guarantees that it won't hit any obstacles on the inside of the planned flightpath.

I guess there would have to be some data collection to actually determine what the most common use cases were, but my general feeling would be that if you asked average users which of the two above behaviours was 'normal', more would choose the latter than the former.

[lthall]

@rmackay9 Yes, sorry. That is already implemented.

@edwinhayes We will only do the corner cut version where the corner is an arc were the connection to the incoming and outgoing track is a tangent to the arc. This mode of cornering should only be used when you are not in danger of hitting anything and represents the shortest and fastest way to follow the waypoints. If you must stay within a fixed distance of the track then you will need to use the second option where waypoint radius is the maximum deviation from the track and the aircraft will slow to be able to achieve the required turn radius.

[andreas-eberle]

@edwinhayes, @lthall: I don't get why there should not be a mode as every dummy user would expect: Follow a the waypoints and get into their radius (allways) and reduce the speed if that is necessary to get in the waypoints radius.

Furthermore, for spline waypoints, it should be perfectly possible to fly the route, as splines maintain by definition a continous speed. It should also be possible to limit the acceleration beforehand to correctly visualize the spline.

I get that this stuff is tricky and @lthall, thanks for working on all that! However, I get the feeling you guys think of expected behavior as the behavior you expect from your code. Of course, this is valid from the developers point of view (your system most of the time cannot offer you all the possibilities of the world), but for a user, this behavior is dangerous. If I get shown a nice route (especially with splines), I expect the aircraft to do exactly that. If it doesn't it might end up in trees. Adding delays is not a good option, as breaking down from 50km/h to 0 km/h is a fucking pain, just do do a light curve.... and my copter definitely has the acceleration capabilities to do such a light (or even harder) turns without shortcutting.

So, while I understand the current system cannot handle this, please aim for something like that for the future.

[lthall]

@andreas-eberle Our concept of expected behaviour is from closely working with a few hundred users to provide the flexibility of mission planning required by them then building the code to meet that requirement. We then provided the flexibility in the mission to access that flexibility. We then expect the user to educate himself on how to achieve their desired path and understand the limitations of their vehicle and settings.

So let me correct a few of your misconceptions: Splines are in no way defined by continuous speed. Here is some background reading that our work was based on: https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Catmull–Rom_spline

So the spline curve can not be followed in all cases with an acceleration limited vehicle at a fixed speed. The distance between waypoints determines how tight the curve is and therefore if we limit the velocity based on the tightest corner then the aircraft could go very slowly the whole mission.

The next thing you should understand is centripetal acceleration: http://theory.uwinnipeg.ca/physics/circ/node6.html

in particular a = v^2/r. So with a maximum lean angle of 45 degrees and a maximum speed of 50 km/h (as you state above) your turn radius is ~20m. You have a waypoint radius of 2m. So no your copter can't go around a corner and stay within 2m of the waypoint without slowing down.

So I hope now you have a little more appreciation of the complexities of navigation and the basic physic and maths involved. Please take the time to understand the limits of your vehicle and the way the navigation works so that you are able to safely design your missions (and stay out of trees).

[andreas-eberle]

@lthall: From my understanding (and reading your linked article), the splines are defined to have a continous (or steady) tangent at their connection points. Based on that a tangent is the derivation of a function, I think it is correct to consider the tanget as the speed, because the speed is the derivation of the place function. Therefore the speed (derivation/tanget of the space function) is continous at the connection points of the splines. Furthermore, cubic splines are cubic functions, so they are continously differentiable. Thus, the derivation (speed) is also continous in the function segment. But I might be wrong here, I had my math lectures some years ago...

However, this does not mean that the speed might be constant. And yes, your totally right, my copter cannot do a 90 degree turn in two meters from 50 km/h, I did not want to imply that.

Maybe we are a talking a bit at cross purposes here. What I'd like to have is a way to fly a route where the waypoint criteria is more important than the set speed. I think for many, if not most use cases, the route is more important than the speed (e.g. talking images, dropping stuff). Thus, I'd whish for a system that is breaking as little as needed to be able to hit the waypoints in the given radius. That this might not be easy, I'm pretty sure about.

[lthall]

Yes, that is specifically the enhancement I was talking about where the aircraft would slow just enough to touch the waypoint radius on the way past while using the maximum acceleration limit set by the user.

This requires that we calculate the maximum turn radius for a given corner that touches the waypoint radius. We then have to slow the aircraft before we reach the start of the turn. We then have to deal with the fact that we are jerk limited and therefore can't go instantaneously from breaking to turning.

The devil is in the details but we will get there.

[andreas-eberle]

Nice to hear that 👍

[edwinhayes]

This engendered more discussion that I anticipated. @lthall, one more question: if in the future, as proposed above, there are two options for cornering behaviour (plus stopping at the WP, which is somewhat different I guess), how will this behaviour be selected? Namely: something which is selectable on a per-waypoint basis, or something which needs to be consistent throughout the mission?

[lthall]

Not sure @edwinhayes. It is one of those things we would need to discover as we go. It depends on what we can fit into mavlink and probably a few other factors.

[lthall]

Related issue that should be addressed simultaneously

[andreas-eberle]

Is there an update on the status of this issue?

[jasonbeach]

We noticed this same issue when we upgraded to Copter 3.5.2. We have sort of hacked around it by giving each waypoint a delay of 1 second as suggested, but also increasing the waypoint radius so that by the time the vehicle actually gets to the waypoint the delay is already satified (if I remember right the delay timer starts counting as soon as the carrot is within the waypoint radius). Ugly hack, particularly when you're sending auto-generated waypoint lists, but it appears to give reasonable results.

I'm kind of surprised that the delay value is constrained to be whole seconds. The mavlink message that carries the parameter is a float--it's only in the class that actually stores and uses that param that casts and stores it as an int.

[andreas-eberle]

Is it possible to change the parameter to accept float values? Then we could do the workaround with a tiny delay value that isn't even noticable.

[rmackay9]

@andreas-eberle, the issue with accepting the float value is that the mission items are stored in a fixed length structure. i don't think there is room in that structure to allow changing the delay to a float.

[andreas-eberle]

@rmackay9: What size (bytes) is the current value? 16 or 32? In both cases, you might be able to store a float with the same bit size in that spot. Could this be an idea?

[rmackay9]

@andreas-eberle, it's not a single field so much as the entire command is stored in a fixed length array. It is 15bytes (AP_Mission.h).

[tizianofiorenzani]

You should mention that even the Position XY parameter has a big impact on the leash length (POS_XY_P for 3.5.5 and older, PSC_POSXY_P for newest). The bigger the parameter the tighter the leash

[Jim-Dow]

Why not just use gently curved, convergent, non-traditional (non-linear/non-parallel) flight lines and avoid the issues related to corner waypoints - along with all the other associated issues. Here's a fixed-wing drone's curved mission plan as an example.

[bretonjustin]

Will the problem be fixed? We ave the same issue where mission time is critical and we cannot afford to stop at every waypoint... Thanks

[rmackay9]

This is a duplicate of this existing issue so I'll close this one. https://github.com/ArduPilot/ardupilot/issues/6194