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

Closed
wants to merge 3 commits into from
+219 −7

## Conversation

Contributor

### brnjones 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:

### How to Use

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

# 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.

Contributor

### rmackay9 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.
Contributor Author

### brnjones 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!
Contributor

### tridge 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
Contributor

### arthurbenemann commented Mar 10, 2013

 I have implemented the run-time selection, I'm finishing a simulation as we speak.
added a commit to arthurbenemann/ardupilot that referenced this pull request Mar 11, 2013
``` Plane: Adding a parameter to hold the L1 reference distance at runtime. ```
`For the code discussed in issue ArduPilot#101`
``` 27b3fe1 ```
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```
``` 5a016bd ```
Contributor

### arthurbenemann 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)

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```
``` 57e4b57 ```
``` 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.```
``` 6349449 ```
``` L1_Control: Added support for turn direction. 1 for clockwise, -1 for… ```
`… counter-clockwise`
``` 28186a1 ```
Contributor

### tridge commented Apr 16, 2013

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