Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane: L1 Control for Straight and Curved Path Following #101

Closed
wants to merge 3 commits into from

Conversation

@brnjones
Copy link
Contributor

commented Feb 24, 2013

This is an implementation of a guidance algorithm described in A New Nonlinear Guidance Logic for Trajectory Tracking (AIAA 2004-4900). This provides controlled path following around curves and straight lines, simplified user setup, and architecture extensible to future higher level path generation algorithms.

The control law consists of 2 parts, 1) Calculation of a reference waypoint (L1_ref) along the trajectory to be followed, and 2) Calculation of a lateral acceleration command that intercepts the calculated L1 reference point. For aircraft, this lateral acceleration command is equivalent to a bank angle command.

Major advantages to the L1 control law are:

  • Reduction of number of configurable gains: Nav_Roll PID and xtrack error gain are replaced with an insensitive L1 reference distance. This largely eliminates tuning for path following.
  • Improved disturbance rejection and circular path tracking in wind
  • Extensible to future higher level trajectory generation algorithms, follow-me, 4DT, vector fields, obstacle avoidance, formation flight, etc. Behavior can be updated by introducing another L1 reference point calculation function.

Implementation

First, a reference waypoint called “L1_ref”, is calculated within the navigation control loop. L1_ref is placed differently depending on straight line or circular following. Furthermore, L1_ref is placed differently depending on which region the aircraft is located. Once the L1_ref point is calculated, the angle between the velocity vector and desired path is calculated. This is performed at 10Hz in navigation.pde. In attitude.pde, a bank angle command is calculated to put the aircraft on a circular path to intercept the L1_ref point.

In essence, the aircraft follows a reference point that constantly slides along the desired path. The method for calculating L1_ref for straight line and circular path following are shown below:

Waypoint_diagrams1

Waypoint_diagrams2

How to Use

To enable L1_Control for loiter and straight line following add the following to APM_Config.h:

define L1_CONTROL ENABLED

define L1_REFERENCE_LENGTH [length (meters)]

With the APM1 or 2 on a Skyfun at ~20 meters/sec, a L1 reference length of 40 meters works well.

@rmackay9

This comment has been minimized.

Copy link
Contributor

commented Feb 25, 2013

This is similar in some ways to the way we're doing waypoints for arducopter now. leonard hall designed all that code for AC.

@brnjones

This comment has been minimized.

Copy link
Contributor Author

commented Feb 25, 2013

@rmackay9 Yes, it definitely shares the concept of navigating to a point moving in a way that places the vehicle on the desired path. I did review Leonard's code in Arducopter, cool stuff is happening!

@tridge

This comment has been minimized.

Copy link
Contributor

commented Mar 10, 2013

Hi Brandon,
I think this is very interesting! I like the structure a lot, although I think I'd like it to be selectable at runtime, so we get more people using it.
I'd like to start a discussion on this on the drones-discuss list, and get Paul Riseborough involved. He's been experimenting with some similar ideas.
I'll start a discussion and see how it goes.
Cheers, Tridge

@arthurbenemann

This comment has been minimized.

Copy link
Contributor

commented Mar 10, 2013

I have implemented the run-time selection, I'm finishing a simulation as we speak.

arthurbenemann added a commit to arthurbenemann/ardupilot that referenced this pull request Mar 11, 2013
arthurbenemann added a commit to arthurbenemann/ardupilot that referenced this pull request Mar 11, 2013
Plane: Using the L1 reference distance parameter at runtime to setup …
…the L1 navigation controller.

This way the L1 controller can be switched on and modified in flight, setting the paramter L1_dist to zero disables the controller and uses the old PID for navigation. This commit is part of issue ArduPilot#101
@arthurbenemann

This comment has been minimized.

Copy link
Contributor

commented Mar 12, 2013

I added a parameter to enable and configure the L1 controller on the commit: 5a016bd

I added some simulations at: drones-discuss. Here is one of the results:

Flight 2 - 8 m/s wind from north (Blue is L1, Yellow is PID)

Teste 2

brnjones added 3 commits Feb 5, 2013
AP_L1_Control: Addition of library for geometry calculations required…
… for L1 Control.

calc_L1_circ: generates L1 reference point on circle
calc_L1_line: generates L1 reference point on line
Plane: Integration of L1_Control for straight line and circular navig…
…ation. L1_Control is a non-linear navigation controller that does not require tuning and provides curved path following.

To enable L1_Control for loiter and straight line following add the following to your APM_Config.h:
#define L1_CONTROL ENABLED
#define L1_REFERENCE_LENGTH [length]
where [length] is a reference length in meters.  The smaller the value the higher gain.  Try 2*groundspeed.
@tridge

This comment has been minimized.

Copy link
Contributor

commented Apr 16, 2013

This is merged in master now - thanks for a fantastic contribution!

@tridge tridge closed this Apr 16, 2013

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
4 participants
You can’t perform that action at this time.