Mavlink Path Planning Protocol support in ArduPilot

[sieuwe1]

Mavlink has already for some time now implemented a Path Planning Protocol to support a companion computer to send trajectories for the vehicle to follow. See more info here:

https://mavlink.io/en/services/trajectory.html

We are working on a system to autonomously avoid obstacles for multi rotors. Our path planning system outputs 5 trajectory points in position and attitude in the local frame. We want to send this trajectory using the mavlink message to ArduPilot so that ArduPilot can perform the actual trajectory.

Currently ArduPilot / ArduCopter does not support trajectory following. Is anybody working on this? Or is there another way to make ArduCopter follow trajectories in the local frame?

The absence of this feature is making us look at PX4 since they have implemented this feature. However we have had good experience with ArduCopter for a while now and it seems that PX4 is less stable in holding posission.

Thanks

Sieuwe

[rmackay9]

Hi @sieuwe1,

Thanks for the enhancement request. I think we can add support for this. Is it just the TRAJECTORY_REPRESENTATION_WAYPOINTS message that you need or others as well?

[sieuwe1]

Hi @rmackay9

For us the TRAJECTORY_REPRESENTATION_WAYPOINTS message is sufficient.

I am no control expert, but I would think that inside the control implementation, a curve has to be fitted to the waypoints to create a smooth trajectory to follow anyway. So implementing the TRAJECTORY_REPRESENTATION_BEZIER would not be much more work.

It seems that the implementation of PX4 supports a max message frequency of 5hz. This is quite low, especially for faster more agile obstacle avoidance. It would be nice to see higher frequencies like 15 or 20hz. Our control loop for example runs on a Nvidia AGX Xavier and can hit up to 20hz.

Our team is not well known with the ArduPilot code base, so contributing support ourselves would be quite difficult. However we can of course contribute by testing out the implementation in simulation and real flights.

We have a couple high end 16 inch quads here which we use for testing. They are great for testing features like this.

[lthall]

Can I ask why you don't use the existing guided mode inputs and send position (and velocity if needed) commands at 5 to 20 Hz?

If you do have 5 specific points you want to go to, how do you need these points to be linked? For example with normal waypoint missions the tricky bit is defining how the corners are created. That is why we have waypoint radius and acceleration limits. This lets us work out when we need to slow down and when we can maintain speed.

[timtuxworth]

Can this be added so it will work for Plane?

[lthall]

Some of the new acro stuff may be applicable with plane. The trick with these problems isn't defining the path, it is handling the fact any practical path is limited by the physics of the aircraft and generally the simple path defined by the message is impossible for the aircraft to follow. For example a multirotor can't travel through a corner between straight lines without coming to a complete stop or cutting the corner and leaving the lines. Plane has less flexibility on how these things are handled so it makes it a little more difficult.

[timtuxworth]

Thats kind of why I mentioned it. This is a problem crying out for a solution, and I know it's not easy, but at very least it would be great if it could be solved in such a way it doesn't make it harder to implement for plane. Of course you know this, but for others reading - it's very important that QuadPlane is also "Plane", so while a QuadPlane has copter like options available for object avoidance, if the base solution doesn't work for Fixed Wing Plane, it may be difficult (for example) to have a QuadPlane flying fixed wing, automatically transition to MultiRotor mode to come to a complete stop if necessary to avoid something.

[sieuwe1]

Hi @lthall

The main reason we want this feature is because our path planning system is designed to work in very complex environments. For example forests and urban environments. In these environments precise control is key. A well written trajectory controller will take into account the curve of the entire trajectory. Lets say the first 3 points are in a straight line, but the last two points create a massive curve. The trajectory controller can already take into account that a sharp curve is coming and that it should already lower its velocity for the curve. This will give much more precise control which we need for our use case.

To be more specific, our path planner gives 5 x, y, z positions in the local frame and 5 corresponding yaw angles. The flight controller is free to change attitude in the other axis to reach needed velocities to perform the trajectory.

For our use case it does not matter at what velocity the drone is at each specific point. The drone should not go to fast to overshoot the points of course and we would like to set a "max speed" which can be lower than what the drone is capable of handling.

Paths that come out of our path planner are generated from earlier flights and would thus be very similar to what the drone is already capable of doing. I think that many path planning systems already take into account what possible paths are. I don't think a good path planner would for example ask for a trajectory with angles between points of 90 degrees. But I could be wrong in this. For our use case it would be fine if the trajectory controller fits a curve to the points which best resembles the trajectory while still being able to fly.

[lthall]

It sounds like this could be a simple 5 waypoint mission using S-Curves and Splines. WP_Nav controller makes all the decisions you describe here and generates a possible kinematic path. You just need to specify this using guided mode.

This does mean that you can't update this path at 5 Hz because that means you can change your mind about how sharp the next corner is making it impossible to follow your desired path. You would need to specify the path and then wait for it to complete.

This would also mean you can't start this path at any speed unless you can slow down before the first corner. So if you can't stop before your first corner you couldn't guarantee you could follow the path.

Currently our real time s-curve kinematic path generator is what guided mode uses to follow high update rate position, velocity, acceleration curves. It can handle fast update rates and the user changing their mind but it also means the user has more intimate control and therefore more responsibility to define the path the way they want. This really fine control sounds like it is more than you want. (Mavlink: SET_POSITION_TARGET_GLOBAL_INT )

So if you could theoretically define a 5 waypoint mission using waypoints and splines, standard auto mode mission commands, would that be able to achieve the path needed? If not, what is missing there.

I realise that sending a mission, changing to auto, then waiting for the mission to finish is not the way you want to work with the autopilot but I am trying to better define exactly what the requirements are for you when it comes to implementing the above mavlink messages.

[sieuwe1]

Hi @lthall,

The SET_POSITION_TARGET_GLOBAL_INT accepts 1 position input right? What we could of course do is to loop over our 5 point trajectory and sending these one by one to the flight controller using the SET_POSITION_TARGET_GLOBAL_INT message. We send a point and wait until the point is reached and then send the next point. However then the controller wont know the entire trajectory and thus cant fit a curve to the entire trajectory to take into account the upcoming points which may for example take a sharp turn.

Also maybe for clarification the entire length of our 5 point trajectory is maximal 3 meter. We are talking about really short and precise trajectories.

A important feature when implementing trajectory support should be that the trajectory can be overwritten by a new trajectory. While following the trajectory our sensors might be able to see a obstacle they were not able to see before and thus we want to alter the trajectory to avoid the new obstacle.

I think the biggest difference's between a 5 point auto mission and a 5 point trajectory message is: 1- The length of the entire path. A 5 point trajectory is a short section of an entire mission. All trajectories (could be hundreds) combined form a mission. 2- The update rate of the message. A 5 point auto mission is send to the drone and it could take minutes for the drone to finish the mission and then a new one can be send. A trajectory is short and should be changed in flight. So they can be updated multiple times a second.

Maybe another way to explain it is that in path planning there are two types of planners. A global planner and a local planner. A global planner gives the general direction (Fly from house A 100m to bridge B). The local planner changes the path of the global planner according to situations that happen along the way (For example fly a curve around a tree in between the house and the bridge). The global planner is a mission in auto mode and the local planner is a trajectory (or multiple trajectories combined).

I think trajectory message support is a really specific feature that would only be used by users who are developing a low level path planning system in combination with a drone for use cases like acrobatics or autonomous obstacle avoidance. If a user wants to fly a drone from A to B they can just use the auto mission function. But since obstacle avoidance on drones is something that a lot of people are interested in, I think its worth implementing.

[lthall]

There is a fundamental problem here that people always struggle to understand. You can't simultaneously have a trajectory that relies on you planning ahead AND let it be changed at 5 Hz.

I think the biggest difference's between a 5 point auto mission and a 5 point trajectory message is: 1- The length of the entire path. A 5 point trajectory is a short section of an entire mission. All trajectories (could be hundreds) combined form a mission.

The point of this question was to understand the functionality. One of the big improvements in S-Curve based auto missions is it is much more scalable. You can do both very small and precise control, by this I mean 10 cm zigzag followed by 100 km legs and it will respect and follow the detail, slowing down as needed, at both scales.

I will try to explain the fundamental limitation that needs to be addressed here.

If you provide a 5 segment trajectory for the aircraft to follow. The aircraft will accelerate to a speed that it can safely follow that trajectory that accounts for the corner it is approaching. If you change that trajectory the aircraft may now be traveling too fast to follow the new path. So now the autopilot would need to create a third path to stop, turn around and get to the beginning of the path you just set.

You can't define a 5 segment path AND update it at 5 Hz because the autopilot would be spending all it's time just trying to get to the start of the path (or initialise the new path)

I hope I sort of explained what is NOT possible. What IS possible? You can do a 5 segment path that progressively gets shorter as the autopilot moves through the path. The user can provide new segments to the end of the path to extend the path. This is possible because any additional path segment will only ever let the aircraft increase in velocity but never force it to decrease in velocity.

The solution Randy has suggested would be to use S-Curves to produce a "Micro Mission" in guided that could be used to deal with these small paths. The restriction on this is you could only add or change points that don't change the velocity of the aircraft at that moment. For example, if you change the point the aircraft is currently moving too, the aircraft would need to calculate it's stopping point then set that as it's next point followed by your new point (not the same as your intended path).

Does this sound like the solution you are thinking of?

Another point that may also help clarify the issues. You said this when talking about SET_POSITION_TARGET_GLOBAL_INT. "However then the controller won't know the entire trajectory and thus cant fit a curve to the entire trajectory to take into account the upcoming points which may for example take a sharp turn." Yes this is true. The autopilot assumes you want a perfectly sharp corner so it plans to come to a complete stop providing you the ability to follow the line with zero error if you require (the only way to go around the corner of two straight lines is to come to a complete stop, any thing else requires you to cut the corner and leave the lines). However, you are able to provide the next point when you can get to that point without hitting your obstacle.

[sieuwe1]

@lthall, Thanks for your explanation and it was indeed already something I was aware of. Maybe it is interesting to look at how PX4 has implemented it? I searched a bit trough the code but could not find the implementation.

I think your solution with "a 5 segment path that progressively gets shorter as the autopilot moves through the path." sounds like a good option.

I think adding something like this below would be a good solution while still having 5hz

• When the first message is received the 5 point trajectory is started by the autopilot. (during flight to the first point, other messages are ignored)
• When the drone reaches the first trajectory point it is removed from the list. When a new message comes in, the last point of the new message is appended to the current trajectory. (since the other points of the trajectory will be overlap)
• This keeps going on until no more messages are received and the trajectory is finished.

However I still think it would be nice to update the existing trajectory. Maybe we can alter the last two points? Or maybe we can say that modified trajectory points are only accepted if they are within a viable range for the current velocity of the vehicle.

I think it would also be nice to have some other people have a look at this.

[lthall]

PX4 don't have the advanced tools we have in ArduCopter to generate paths, much less the ability to follow them accurately. So their implementation, if they even have one, is not going to be helpful.

I think it would also be nice to have some other people have a look at this.

We have 3 people looking at this problem and I think we have a way forward.

[sieuwe1]

A great! Can you send a update here once the details are worked out? Then we can already start implementing some code to test the feature.

[lthall]

I just read the explanation again. The TRAJECTORY_REPRESENTATION_WAYPOINTS is used communicate the AutoPilots waypoints to the companion computer. It is only used to provide a single setpoint back to the Autopilot.

The companion computer then provides the setpoint to the aircraft by populating just the first point in each array.

These setpoints are used to override the internal navigation of the autopilot when an obstacle is detected. When the autopilot stops receiving commands it continues to navigate normally.

Now I understand how PX4 can do this LOL! It isn't doing it, the companion computer is doing everything. The message is used to tell the companion computer what the next 5 waypoints are in the autopilots normal navigation. The companion computer then looks at this path and works out if there is an obstacle along this path. If it detects an obstacle that needs to be avoided it sends the equivalent of SET_POSITION_TARGET_GLOBAL_INT messages at a high update rate until the obstacle is avoided.

[sieuwe1]

@lthall

LOL I see. Kind of a strange way to support this message type. This also means they don't really have the smoothest possible control for their obstacle avoidance since there controller just flies from point to point without knowing the shape of the entire trajectory?

Implementing something better for this message type in ArduPilot can be a big advantage for a lot of researcher (like us) working on path planning and drones.

A good solution would be a mix between the customizability of SET_POSITION_TARGET_GLOBAL_INT and the smooth S-curve control policy of a waypoint mission. The main problem I currently see with using a waypoint mission in our use case is that it can not be changed in flight. I definitely understand the inherent issue with having a controller that plans ahead while the path might be changed while flying. However I think there should be a good solution for this. I cant imagine a system like Skydio, just plans a path and goes with it in the hope it is actually correct.

Having a interface in guided mode that works like this would I think be a good solution.

@lthall, Thanks for your explanation and it was indeed already something I was aware of. Maybe it is interesting to look at how PX4 has implemented it? I searched a bit trough the code but could not find the implementation.

I think your solution with "a 5 segment path that progressively gets shorter as the autopilot moves through the path." sounds like a good option.

I think adding something like this below would be a good solution while still having 5hz

• When the first message is received the 5 point trajectory is started by the autopilot. (during flight to the first point, other messages are ignored)
• When the drone reaches the first trajectory point it is removed from the list. When a new message comes in, the last point of the new message is appended to the current trajectory. (since the other points of the trajectory will be overlap)
• This keeps going on until no more messages are received and the trajectory is finished.

However I still think it would be nice to update the existing trajectory. Maybe we can alter the last two points? Or maybe we can say that modified trajectory points are only accepted if they are within a viable range for the current velocity of the vehicle.

I think it would also be nice to have some other people have a look at this.

Another option could be to define a 5 point waypoint mission in guided mode which the controller starts to perform. During flight the companion computer discovers that the mission will cause a collision. The companion computer sends a message which immediately stops the drone. While slowing down the controller can already accept a new 5 point waypoint mission which it can follow.

Having feedback from the controller about its current planned path like PX4 has would also be a nice feature. We wont be using something like this soon, but we can see this becoming useful in the future for our system.

Thanks

Sieuwe

[sieuwe1]

Hi,

There has not been any activity in this issue for some time. Is this still something that the ardupilot team is working on? Or is there already a solution in ArduPilot that supports short missions which can ideally be changed during flight?

Thanks

Sieuwe

[lthall]

When/if we implement support for this message it would be as defined in the documentation.

Up to the next 5 waypoints are generated by the autopilot.

One waypoint consisting of a position, velocity and acceleration will be accepted and when received used to generate a trajectory in the same way as SET_POSITION_TARGET_GLOBAL_INT with position, velocity and acceleration.

It will be up to the companion computer to calculate the desired trajectory and repeatedly generate a new waypoint 1 data set to follow.

However, given our discussion above this message as it is defined in the documentation does not do what you hoped. As there doesn't appear to be any interest in this message I can't see it being made a priority.

[rmackay9]

I've still got this on my to-do list to implement but I can't promise when I'll get to it..

[samshadnc]

Hi, i am also working on trajectory optimisation for UAV, and I am using Arducopter with Mavros for controlling the drone. Even mavros support trajectory publish option Ardupilot not responding to this. Is someone used this feature with Arducopter. any healp will be geatfull. @rmackay9 please share if you have any update on this topis

[afonsoVale]

Hello! Are there any news on the implementation of this feature? It would be very interesting to be able to use it, at my university a few students are working on algorithms for navigation in unknown environments for example and it would be quite cool to be able to command trajectories using this, rather than having to rely on external controllers for it.