Trajectory generator for Auto mode #10746

bresch · 2018-10-23T15:41:18Z

Based on #10696
Needs #10772 to work with VTOL planes.

Overview

This new FlightTask uses the velocity-based trajectory generator developed for manual control in #10696 to generate a smooth trajectory between waypoints. The strategy is quite simple: for each axis (north-east-down), a velocity target is generated using simple proportional controllers: v_target = K * (x_waypoint - x_trajectory) where x_... is the position. The controller then "drives" the trajectory to the waypoint like a servo controls its position by controlling the velocity of the motor. Furthermore, to ensure that the trajectory asymptotically converges to the line connecting the waypoints, a cross-track controller is added (here also, a simple proportional controller is used).

To robustify the algorithm, a time stretch strategy is used:
At each epoch, the increasing acceleration, constant acceleration and decreasing periods are computed. To generate the trajectory, the jerk is integrated using dt*time_stretch where time_stretch is a value between 0.0 and 1.0. This time stretch value is computed as a function of "how far the drone is behind the position of the trajectory". In summary, we can say that the trajectory will wait for the drone if it's too far from it.

Takeoff and landing

The advantage is that for takeoff and landing waypoints, the trajectory is still continuous. The new AutoMapper2 flight task is used to provide proper position and velocity setpoints given the type of waypoint:

Takeoff: xyz position of the setpoint and z velocity limit depending on the altitude above ground
Landing: xy position and z velocity limit depending on the agl

The interface used is similar to the basic FlightTask idea:

a value in the position setpoint means that the velocity target has to be generated by the P controller, a NAN means that the velocity target is given by the velocity setpoint message.
a value in the velocity setpoint means that:
- if position setpoint is NAN then this is the velocity target
- if position setpoint is finite then this is the maximum allowed velocity target
if both velocity and position setpoints are NAN, then the velocity target is set to zero.

Tests

Raw setpoints

Position along North axis:

Generated path:

Using this trajectory generator

Position along North axis:

Generated path:

How to use it

Set MPC_AUTO_MODE to 1
Set MPC_YAW_MODE to 3 (coordinated turns)
Tuning parameters:
MPC_ACC_HOR is used as the maximum acceleration/deceleration for x and y
MPC_JERK_MIN is used as the maximum jerk for x, y and z
MPC_XY_CRUISE is the cruise speed, the max velocity of the trajectory
MPC_XY_VEL_MAX is the max velocity at the input of the velocity controller (has to be bigger than the cruise speed to allow the position loop to do its job)
MPC_ACC_DOWN_MAX
MPC_ACC_UP_MAX
MPC_Z_VEL_MAX_DN
MPC_Z_VEL_MAX_UP
MPC_XY_TRAJ_P controls the "aggressiveness" of the horizontal trajectory.
MPC_Z_TRAJ_Pcontrols the "aggressiveness" of the vertical trajectory.

ndepal · 2018-10-23T16:28:52Z

Cool! I see that this causes the vehicle to deviate quite far from the waypoints and the straight lines connecting them. What guarantees can be made about maximum deviation of the vehicle position from the straight lines? How hard will it be to predict the actual trajectory ahead of flight (e.g. drawing the red lines from above in QGC as the waypoints are placed)?

bresch · 2018-10-24T11:55:31Z

Hi, @ndepal
The tracking can now be adjusted with the MPC_XY_TRAJ_P and MPX_Z_TRAJ_P gains. To adjust the distance at which the drone starts to turn, the acceptance radius had to be adjusted. However, the acceptance radius being fixed, an acute angle might generate an overshoot due to the maximum dynamics of the trajectory (velocity, acceleration and jerk limitations.
Below a plot with the MPC_XY_TRAJ_P gain slightly increased and with smaller acceptance radius.

(log)

As it directly depends on the parameters of the trajectory and is completely independent of the vehicle's dynamics, the trajectory (and path) is deterministic. The green line from above could then be displayed on QGC.

dagar · 2018-10-24T14:42:19Z

CI build failures.

ndepal · 2018-10-25T07:33:29Z

Thanks for the explanation @bresch.
Being able to show the green line in QGC could be valuable, as it can deviate significantly from the straight lines. However, the actual vehicle trajectory deviates quite a bit from the setpoints as well. Is this by design or a question of position control tuning? This cam make it quite difficult to plan missions, especially when the mission is generated algorithmically.

A while ago there was an effort to implement spline/bezier trajectories. These would be smooth and feasible trajectories, allowing for a controller that closely tracks the planned path. And there are algorithms for planning such paths in an obstacle map. Do you know what the plans are in that regard? Does this PR replace those efforts?

bresch · 2018-10-25T08:47:56Z

@ndepal The bad tracking is mostly due to the poorly tuned model I have in jMAVSim. The tracking will also be improved by adding the acceleration trajectory as a feedforward to the velocity controller.

I'm not 100% sure, but I think the spline/bezier trajectories you're mentioning are in fact paths and not trajectories.

dagar · 2018-10-25T14:40:02Z

We'll need to figure out something to get this to fit in px4fmu-v2, but in the meantime let's test on other platforms.

@PX4/testflights please test this on a few multicopter platforms that aren't px4fmu-v2.

bresch · 2018-10-25T14:49:56Z

@PX4/testflights Could you please test that on a quad? Something like a F450
It should be a small mission, possibly a survey, for example like this one: https://logs.px4.io/plot_app?log=bf80702d-7046-4e56-9669-51bb7a1c841a

MPC_AUTO_MODE = 3 (enable trajectory generator)
MPC_YAW_MODE = 3 (generate yaw from trajectory velocity vector)

The following parameters should be good for a small quad:
MPC_ACC_HOR_MAX = 3
MPC_JERK_MIN = 5
MPC_XY_CRUISE = 5
MPC_XY_VEL_MAX = 12
MPC_ACC_DOWN_MAX = 5
MPC_ACC_UP_MAX = 8
MPC_Z_VEL_MAX_DN = 2
MPC_Z_VEL_MAX_UP = 3
MPC_LAND_ALT1 = MPC_LAND_ALT2 = 5
MPC_LAND_SPEED = 0.7
MPC_XY_TRAJ_P= 0.5
MPC_Z_TRAJ_P= 0.5

Thanks in advance!

Avysuarez · 2018-10-25T20:12:57Z

I tested in pixhawk 4 and pixhawk 4 mini. in both, there is no difference with the master.
Pixhawk 4 logs
https://review.px4.io/plot_app?log=6d9a6c04-6e5f-492b-9b8a-f98e23ff3239
https://review.px4.io/plot_app?log=ca2a2a35-2d5b-4184-9e09-9e3fedbc3f53

Pixhawk 4 mini logs.
https://review.px4.io/plot_app?log=c232a5e9-e6b2-43f1-9a5a-526ea788d7ba
https://review.px4.io/plot_app?log=f8f78a60-d580-44e7-b0c7-e344eb8cc127

bresch · 2018-10-25T20:33:02Z

@Avysuarez Thanks! Could you also try with those specific parameters please: #10746 (comment)
The jerk and accelerations are set too high, that's why you don't see the difference :)
I'll maybe set those as default in the future.

Avysuarez · 2018-10-25T20:34:39Z

@bresch
ok I will try again

bresch · 2018-10-26T09:25:41Z

With MPC_YAW_MODE = 3 : https://logs.px4.io/plot_app?log=5a7d2330-324e-4059-8ab7-c8f971b57ee7

bresch · 2018-10-26T10:02:59Z

rebased on Master

Avysuarez · 2018-10-26T17:37:41Z

I tested again with the same vehicles but with the suggested parameters. The flight is smoother.
Pixhawk 4.
https://review.px4.io/plot_app?log=4ae93006-95ae-4c09-96b1-0b66f1fb31bf

Pixhawk 4 mini.
https://review.px4.io/plot_app?log=f6bbcc65-59d1-4d6c-a85c-98095cc76be8

bresch · 2018-10-26T18:11:58Z

@Avysuarez Great! Thanks a lot.

dannyfpv · 2018-10-26T19:55:01Z

tested on pixhawk v4pro, following parameters.
https://review.px4.io/plot_app?log=1b1576ef-1a38-4a64-96f9-905dd3b9013a
https://review.px4.io/plot_app?log=881213c8-5c1d-4aa2-85a3-5a85b55e7fb7

pixhawk mini
https://review.px4.io/plot_app?log=9cab8e7f-2d9e-419a-afd3-4859d65370aa
https://review.px4.io/plot_app?log=a7c84058-8824-4135-b641-798ed45adad4

bresch · 2018-10-27T07:37:59Z

@dannyfpv Awesome, thanks!
It looks even better than in simulation.

bresch · 2018-10-29T09:05:28Z

Here are some plots of a real flight (extracted from a flight made by @dannyfpv )

Note: the velocity setpoint in the graph above is the sum of the output of the position P controller and the feedforward generated by the trajectory generator.

A general good result is that all the discontinuities have been removed up to the rate setpoints.

Derivation by Mathieu Bresciani: https://github.com/Auterion/trajectory_generator

… getters to public section

…ion period

… Flight Task

- move the update after the integration: a new computed jerk has an impact at the next epoch only - add jerk reduction in case of too large integration time: when a jerk of "min_jerk" during dt is too much - add jerk reduction if the integration time is larger than the predicted one and that integrating that jerk would lead to an acceleration overshoot - rename some variables

… auto mode only

… takeoff)

bresch · 2018-11-02T15:35:33Z

@dagar Rebased on Master.
Here is a mission test in SITL: https://logs.px4.io/plot_app?log=aa76fe5b-5ab7-418c-b15c-a3d2b92bc2fc

bresch · 2018-11-02T15:36:25Z

Could we merge #10745 and #10772 before this PR?

VTOLDavid · 2018-12-03T13:23:56Z

Hi, I'm trying to confingure this mode in master but the parameters to configure it are mising. What I am doing wrong?

bresch · 2018-12-03T21:23:10Z

Hi @VTOLDavid ,
Which parameters are missing? Do you have the latest QGC? Are they not there even if you look for them in the search field?
Most of them are not new.

VTOLDavid · 2018-12-03T22:15:24Z

This parameters do not show up:
MPC_AUTO_MODE
MPC_YAW_MODE
MPC_XY_TRAJ_P
MPC_Z_TRAJ_P
Should I update Qgroundcontrol? I updated it 1 o 2 months ago.
I tried flashing with Qgroundcontrol stable, developer and beta versions and compiling it doing git clone https://github.com/PX4/Firmware.git and make px4_fmu-v3_default it possible that I'm not getting the current version of the master code?

hamishwillee · 2018-12-03T22:26:23Z

Should I update Qgroundcontrol? I updated it 1 o 2 months ago.

Yes. Try a daily build. Though you should normally still see the parameters as long as you are using firmware based on the master codeline (ie param metadata is in firmware, not QGC).

The parameters are in master - you can tell because they show up in the docs here: https://docs.px4.io/en/advanced_config/parameter_reference.html#MPC_AUTO_MODE (QGC firmware gets params from the same source).

VTOLDavid · 2018-12-05T17:28:37Z

Thanks, solved!

bangyanz · 2019-01-26T14:42:44Z

@bresch I am a bit confused about your _generateTrajectory() code. It looks like you computed the desired velocity_setpoint in _prepareSetpoints(). But you immediately change that _velocity_setpoint to vel_sp_smooth in generateTrajectory(), which does integration based on the jerk and vel_prev. I failed to see how the computed _velocity_setpoint factors into the new vel_sp_smooth. Can you explain that?

bresch · 2019-01-28T09:35:10Z

Hi @bangyanz ,
The _velocity_setpoint in _prepareSetpoints is the "desired/target velocity", before smoothing. In _generateTrajectory, this desired velocity is used as an input to the algorithm here.
The variable _velocity_setpoint is then re-used to carry the result of the algorithm (vel_sp_smooth) back to the controller.

bresch added Admin: Enhancement (improvement) 💡 Multirotor 🛸 labels Oct 23, 2018

bresch force-pushed the dev-auto-trajectory branch from 41e89e3 to b851b17 Compare October 23, 2018 15:44

bresch force-pushed the dev-auto-trajectory branch from f478733 to b95323c Compare October 25, 2018 06:38

bresch force-pushed the dev-auto-trajectory branch from 1afd26c to 70bb4a3 Compare October 25, 2018 14:22

dagar requested a review from a team October 25, 2018 14:38

dagar requested review from RomanBapst and Stifael October 25, 2018 15:29

bresch force-pushed the dev-auto-trajectory branch from 9edaf84 to 1631789 Compare October 26, 2018 10:02

bresch removed the request for review from a team October 29, 2018 09:58

dagar mentioned this pull request Nov 1, 2018

px4fmu-v2 disable more drivers #10799

Merged

bresch mentioned this pull request Nov 2, 2018

Smooth takeoff in auto considerations #8885

Closed

Flight Tasks: add new trajectory smoothing flight task & library

4a71975

Derivation by Mathieu Bresciani: https://github.com/Auterion/trajectory_generator

bresch added 11 commits November 2, 2018 16:16

Trajectory - Add getters for current position and velocity. Move some…

4ea35b1

… getters to public section

Trajectory - Overload integrate function to allow for custom integrat…

0b33b9d

…ion period

Trajectory - format style

40f9397

Vel smooth - Improve position lock/unlock detection

368fe39

Auto Smooth Vel - Add AutoMapper2 and AutoLineSmoothVel flight tasks

d042c27

Auto Traj - Disable reActivate "reset" function for AutoLineSmoothVel…

7811a02

… Flight Task

Auto traj - Add parameter for gain of trajectory controller

6ac5931

Auto traj - Add dedicated parameter to enable trajectory smoothing in…

7a853ee

… auto mode only

Auto traj - generate heading from trajectory velocity vector if possible

96d6429

Manual Smoth Vel - Override reActivate to not reset Z axis (prevented…

22d178a

… takeoff)

bresch force-pushed the dev-auto-trajectory branch from c9b19a6 to 22d178a Compare November 2, 2018 15:34

RomanBapst mentioned this pull request Nov 6, 2018

Velocity setpoint jump in auto mode #8863

Closed

RomanBapst approved these changes Nov 6, 2018

View reviewed changes

RomanBapst merged commit 90cee2d into master Nov 6, 2018

RomanBapst deleted the dev-auto-trajectory branch November 6, 2018 21:17

bresch mentioned this pull request Nov 7, 2018

[WIP] Velocity-based trajectory generator for manual position control mode #10696

Closed

3 tasks

MaEtUgR mentioned this pull request Jan 17, 2019

AutoMapper1/2: set thrust to NAN at idle #11229

Closed

bresch mentioned this pull request Feb 27, 2019

Re-add glitch-free position unlock #11549

Merged

MaEtUgR mentioned this pull request Apr 10, 2019

Trajectory mission slow final waypoint approach #11827

Closed

bys1123 mentioned this pull request May 13, 2019

FW altitude control overshoot #10495

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Trajectory generator for Auto mode #10746

Trajectory generator for Auto mode #10746

bresch commented Oct 23, 2018 •

edited

Loading

ndepal commented Oct 23, 2018

bresch commented Oct 24, 2018 •

edited

Loading

dagar commented Oct 24, 2018

ndepal commented Oct 25, 2018

bresch commented Oct 25, 2018

dagar commented Oct 25, 2018

bresch commented Oct 25, 2018 •

edited

Loading

Avysuarez commented Oct 25, 2018

bresch commented Oct 25, 2018 •

edited

Loading

Avysuarez commented Oct 25, 2018

bresch commented Oct 26, 2018

bresch commented Oct 26, 2018

Avysuarez commented Oct 26, 2018

bresch commented Oct 26, 2018

dannyfpv commented Oct 26, 2018

bresch commented Oct 27, 2018 •

edited

Loading

bresch commented Oct 29, 2018

bresch commented Nov 2, 2018

bresch commented Nov 2, 2018 •

edited

Loading

VTOLDavid commented Dec 3, 2018

bresch commented Dec 3, 2018

VTOLDavid commented Dec 3, 2018

hamishwillee commented Dec 3, 2018 •

edited

Loading

VTOLDavid commented Dec 5, 2018

bangyanz commented Jan 26, 2019

bresch commented Jan 28, 2019

Trajectory generator for Auto mode #10746

Trajectory generator for Auto mode #10746

Conversation

bresch commented Oct 23, 2018 • edited Loading

Overview

Takeoff and landing

Tests

Raw setpoints

Using this trajectory generator

How to use it

ndepal commented Oct 23, 2018

bresch commented Oct 24, 2018 • edited Loading

dagar commented Oct 24, 2018

ndepal commented Oct 25, 2018

bresch commented Oct 25, 2018

dagar commented Oct 25, 2018

bresch commented Oct 25, 2018 • edited Loading

Avysuarez commented Oct 25, 2018

bresch commented Oct 25, 2018 • edited Loading

Avysuarez commented Oct 25, 2018

bresch commented Oct 26, 2018

bresch commented Oct 26, 2018

Avysuarez commented Oct 26, 2018

bresch commented Oct 26, 2018

dannyfpv commented Oct 26, 2018

bresch commented Oct 27, 2018 • edited Loading

bresch commented Oct 29, 2018

bresch commented Nov 2, 2018

bresch commented Nov 2, 2018 • edited Loading

VTOLDavid commented Dec 3, 2018

bresch commented Dec 3, 2018

VTOLDavid commented Dec 3, 2018

hamishwillee commented Dec 3, 2018 • edited Loading

VTOLDavid commented Dec 5, 2018

bangyanz commented Jan 26, 2019

bresch commented Jan 28, 2019

bresch commented Oct 23, 2018 •

edited

Loading

bresch commented Oct 24, 2018 •

edited

Loading

bresch commented Oct 25, 2018 •

edited

Loading

bresch commented Oct 25, 2018 •

edited

Loading

bresch commented Oct 27, 2018 •

edited

Loading

bresch commented Nov 2, 2018 •

edited

Loading

hamishwillee commented Dec 3, 2018 •

edited

Loading