[WIP] MC auto mode - Use L1-like logic for lateral line tracking #11841

bresch · 2019-04-12T16:37:27Z

Context

Auto mode uses a model-following tracking approach where the model is composed of 3 integrators with limited outputs (limited jerk, acceleration and velocity). This virtual vehicle is driven from one waypoint to the other using a control loop that generates desired velocities in the local frame. This controller is basically a "virtual pilot" that moves the sticks of the remote.

Current state

So far, a simple 2D proportional controller in the local frame was used to drive the vehicle and gives acceptable results for a first iteration.

L1 approach

A second approach I am trying now is to use the same approach as the fixedwing L1 controller to drive the virtual model.
Basically, a L1 distance of length MPC_L1_DISTANCE is used to compute the position of a virtual point to track on a line defined by two waypoints. The generated vector (limited-jerk model to virtual point) is used to generate the forward speed using MPC_L1_SPEED_P and its direction drives the model to the line asymptotically.
A nice side effect of this controller is that increasing the acceptance radius of the mission items produces a smooth "auto-continue behavior", similar to a fixed-wing flight.

Test data / coverage
I tested that controller extensively in SITL:

A log: https://logs.px4.io/plot_app?log=fa390045-c598-4800-b393-ac22c2cc0812

An other log of the F450:
https://logs.px4.io/plot_app?log=23542ad5-d204-40a3-8787-6b9c31c5756e

Todo

Check if the we can simplify the equations.

mhkabir · 2019-04-12T18:54:38Z

How does this method perform when the acceptance radius is small? (e.g in a survey when cutting corners is not desired)

What happens when the acceptance radius is set unreasonably low? (just thinking about corner cases)

bresch · 2019-04-13T18:35:05Z

@mhkabir The behavior is as good as before with the default radius but I have to check with an unreasonably small radius.

bresch · 2019-04-15T09:04:25Z

@mhkabir
Here is one mission with an acceptance radius of 1.0m:
https://logs.px4.io/plot_app?log=47ec6d55-0305-4096-a33a-8cdae2f1201f

And one with 0.1m:
https://logs.px4.io/plot_app?log=47de542e-0e63-4d60-89a8-c37eed5ad848
(I stopped it because it was too slow; takes too much time to validate each waypoint, but this is to expect, nothing weird)

An other one with 10.0m acceptance and turnaround waypoints:
https://logs.px4.io/plot_app?log=85f7f55b-53ec-43c5-bab8-9b9e5ee89207

limited-jerk model. A L1 distance of length MPC_L1_DISTANCE is used to compute the position of a virtual point to track on a line defined by two waypoints. The generated vector (limited-jerk model to virtual point) is used to generate the forward speed using MPC_L1_SPEED_P and its direction drives the model to the line asymptotically. Increasing the acceptance radius of the mission items produces a smooth auto-continue behavior, similar to a fixed-wing flight.

mhkabir · 2019-04-15T19:49:28Z

Lovely, looks great!

MaEtUgR

Some suggestions to improve. I can do some tests to make sure they work.

MaEtUgR · 2019-04-16T09:26:26Z

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -157,12 +157,32 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 		Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
 		Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
 		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+		Vector2f closest_pt_to_dest = pos_sp_xy - closest_pt;


Comments about the meaning of these individual results would highly speed up code readability.

MaEtUgR · 2019-04-16T09:44:54Z

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

+		}
+
+		// Recompute new L1 length limited by the maximum alongtrack error
+		l1 = sqrtf(crosstrack_error * crosstrack_error + alongtrack_error * alongtrack_error);


Could you not simplify to

l1 = math::max(l1, u_prev_to_dest.length()); alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);

I can test and commit that if you want.

MaEtUgR · 2019-04-16T09:53:00Z

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

+
+		// Position of the point on the line where L1 intersect the line between the two waypoints
+		Vector2f pos_crossing_point = closest_pt + alongtrack_error * u_pos_traj_to_dest_xy;
+		Vector2f u_traj_to_crossing_point = (pos_crossing_point - pos_traj) / l1;


We should use unit_or_zero() like above because otherwise your trajectory state could spoil to NAN when the l1 distance gets zero while you're passing the waypoint.

MaEtUgR · 2019-04-16T14:09:59Z

I did some SITL testing and noticed some points which probably require refinement:

If I put a very high cruise speed e.g. 20m/s and have the default L1 distance it only goes 6m/s max
The default acceptance radius for multicopters currently is 2m: https://github.com/PX4/Firmware/blob/74bc6870ed2a987de9edd987b84080c905b048ab/ROMFS/px4fmu_common/init.d/rc.mc_defaults#L12 and with this default you don't get a result as nice as in your screenshots
If you put a bigger acceptance radius and you plan a moderatly small mission e.g. to avoid trees it will quickly skip points and take bigger shortcuts.
Tuning L1 distance and gain doesn't seem intuitive/"foolproof". I know it's also the case for attitude gains but I tried changing the values slightly and managed to get it flying what seems like random curved patterns and not reaching the waypoint anymore.

I think it's a very good idea but not mergeable yet.

FlavioTonelli · 2019-05-15T16:30:45Z

I tested the new logic on a real flight and I provide you an extra feedback.
When I plan in line waypoints(a straight line with multiple waypoints) the vehicle decrease the speed each time it is approaching a waypoint.
In a fixed-wing like flight I would have except a much smoother behavior.
In my opinion, if I set a large acceptance radius the vehicle has to pass through the intermediate waypoints without slowing down so much.

bresch · 2019-07-05T06:54:47Z

I wasn't able to get a nice behavior with this method, the tracking was too sloppy for a multicopter.

bresch added Admin: Enhancement (improvement) 💡 Multirotor 🛸 labels Apr 12, 2019

bresch requested a review from MaEtUgR April 12, 2019 16:37

bresch self-assigned this Apr 12, 2019

dagar self-requested a review April 12, 2019 17:26

bresch marked this pull request as ready for review April 15, 2019 17:05

bresch force-pushed the dev-mpc-l1-upstream branch from 14bf03d to 4290254 Compare April 16, 2019 08:01

MaEtUgR mentioned this pull request Apr 16, 2019

Trajectory mission slow final waypoint approach #11827

Closed

MaEtUgR reviewed Apr 16, 2019

View reviewed changes

MaEtUgR changed the title ~~MC auto mode - Use L1-like logic for lateral line tracking~~ [WIP] MC auto mode - Use L1-like logic for lateral line tracking Apr 16, 2019

weekly-digest bot mentioned this pull request May 19, 2019

Weekly Digest (12 May, 2019 - 19 May, 2019) #12043

Closed

bresch mentioned this pull request Jul 4, 2019

Auto - Update velocity setpoint generator to avoid overshoot at high cruise speed #12417

Merged

bresch closed this Jul 4, 2019

bresch deleted the dev-mpc-l1-upstream branch July 5, 2019 06:54

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP] MC auto mode - Use L1-like logic for lateral line tracking #11841

[WIP] MC auto mode - Use L1-like logic for lateral line tracking #11841

bresch commented Apr 12, 2019 •

edited by AuterionWrikeBot

Loading

mhkabir commented Apr 12, 2019 •

edited

Loading

bresch commented Apr 13, 2019

bresch commented Apr 15, 2019

mhkabir commented Apr 15, 2019

MaEtUgR left a comment

MaEtUgR Apr 16, 2019

MaEtUgR Apr 16, 2019

MaEtUgR Apr 16, 2019

MaEtUgR commented Apr 16, 2019 •

edited

Loading

FlavioTonelli commented May 15, 2019 •

edited

Loading

bresch commented Jul 5, 2019

[WIP] MC auto mode - Use L1-like logic for lateral line tracking #11841

[WIP] MC auto mode - Use L1-like logic for lateral line tracking #11841

Conversation

bresch commented Apr 12, 2019 • edited by AuterionWrikeBot Loading

Context

Current state

L1 approach

Todo

mhkabir commented Apr 12, 2019 • edited Loading

bresch commented Apr 13, 2019

bresch commented Apr 15, 2019

mhkabir commented Apr 15, 2019

MaEtUgR left a comment

Choose a reason for hiding this comment

MaEtUgR Apr 16, 2019

Choose a reason for hiding this comment

MaEtUgR Apr 16, 2019

Choose a reason for hiding this comment

MaEtUgR Apr 16, 2019

Choose a reason for hiding this comment

MaEtUgR commented Apr 16, 2019 • edited Loading

FlavioTonelli commented May 15, 2019 • edited Loading

bresch commented Jul 5, 2019

bresch commented Apr 12, 2019 •

edited by AuterionWrikeBot

Loading

mhkabir commented Apr 12, 2019 •

edited

Loading

MaEtUgR commented Apr 16, 2019 •

edited

Loading

FlavioTonelli commented May 15, 2019 •

edited

Loading