Re: [ardupilot] Plane: L1 Control for Straight and Curved Path Following (#101)

[Andrew Tridgell]

Hi Brandon,

I've been having a look at your L1 control pull request. Nice piece of
work!

If we adopt this then I think I'd like it to be a runtime selectable
option, so more people can use it without having to rebuild.

I had a bit of trouble tuning it for the Rascal110 in SITL. When I set
the reference length to 40 it weaved like mad. With a ref length of 150
it did much better, but didn't handle wind too well in loiter.

I'm not sure if you know that Paul Riseborough is also working on the
same sort of ideas, and so is Greg Fletcher! We should have a bit of a
discussion on what approach we should use.

Paul, can you have a look at what Brandon has done in this patch:

https://github.com/diydrones/ardupilot/pull/101

and Brandon, can you have a look at what Paul is doing here:

https://github.com/priseborough/ardupilot/commits/master

There is also Gregs patches here which he has been working on for a
while, and produces good results in his HIL testing:

https://github.com/diydrones/ardupilot/pull/123

I think all of you know more about control theory than I do, so it would
be great if you could discuss between you what approaches will work
best.

The key criterion I'm looking for are:

- simple tuning for users
- smooth handling of navigation, without too much overshoot/undershoot
- good handling of wind
- minimal servo jitter (the current master code moves the servos like
crazy in a loiter)
- good mathematical behaviour at extremes (eg. I see some potential for
division by zero in Brandons L1 control patches, we need to fix that)

The tests I tend to do are to fly in SITL with the Rascal110 and a large
rectangular mission loop (I attach a typical test mission below). I then
test with a SkyFun and HotDog at my local flying field. If it behaves
well with all 3 aircraft then I'm happy!

Cheers, Tridge

[Arthur Benemann]

If we adopt this then I think I'd like it to be a runtime selectable
option, so more people can use it without having to rebuild. 

I have just added a parameter to set the L1 reference length, the commits are at this branch or at Github discussion #101. If the parameter L1_dist is set to zero the L1 controller is disabled and the old PID controller takes over. 

This makes testing the controller easy, you just need to add "#define L1_CONTROL ENABLED"  at APM_Config.h. I'm running a simulation in HIL right now and will get back with the results.

[Brandon Jones]

Hi Tridge,

I appreciate your time looking at it, and thank you for the feedback and criterion!  Will look over Paul and Greg's patches too!.

I didn't test with SITL, only HIL and real flight with an APM1 and Skyfun.  So let me get SITL up and running to replicate your experience, since it doesn't reflect what I saw in HIL and real flight test.  Was 150 meters larger than the loiter diameter?

I've also pushed a branch that adds fly-by waypoints, using a turn center, entry, and exit waypoint generation function. It uses AP_Mission and L1_Control:

https://github.com/brnjones/ardupilot/commits/Flyby_WP_type

Concur with possible divide by zero issues, I will address those.  This also relies heavily on gps ground speed, and would like to leverage the ground_speed used in the dead reckoning code you created.  Do you see any issue there?

-Brandon

QGC WPL 110
0       1       0       16      0.000000        0.000000        0.000000        0.000000        -35.362938      149.165085      584.409973      1
1       0       3       22      15.000000       0.000000        0.000000        0.000000        -35.361164      149.163986      28.110001       1
2       0       3       16      0.000000        0.000000        0.000000        0.000000        -35.359467      149.161697      99.800003       1
3       0       3       16      0.000000        0.000000        0.000000        0.000000        -35.366333      149.162659      100.730003      1
4       0       3       16      0.000000        0.000000        0.000000        0.000000        -35.366131      149.164581      100.000000      1
5       0       3       16      0.000000        0.000000        0.000000        0.000000        -35.359272      149.163757      100.000000      1
6       0       3       177     2.000000        -1.000000       0.000000        0.000000        0.000000        0.000000        0.000000        1
7       0       3       16      0.000000        0.000000        0.000000        0.000000        -35.359272      149.163757      100.000000      1

[Arthur Benemann]

Here is the simulations I ran on the L1 code. My setup is as follows:

• APM1 board on HIL_ATTITUDE
• X-Plane9 as the simulator, using PT-60 plane
• Code as in commit #5a016bd
• Parameters for the PID controls as set in the repositories. (config.param attached).
  
• Parameters for the L1 controller set to 40m reference distance, and all others as the file above. (config.param attached)
  
• Mission with 21 waypoints, including a grid, sharp curves and loiter.
• Wind correction was disabled (I think because I had no magnetometer on the simulation)

I ran both codes in the following scenarios:

• No wind
• 8 m/s wind from north (the plane was flying at 15m/s)

The result's are attached (including .tlogs and kmz of the flight), but here are some pictures (for fullsize click on the pictures so it's easy to understand):

Flight 1 - No wind (Blue is L1, Yellow is PID)

On this flight there isn't so much differences between PID and L1, but L1 obviusly tracks the lines better. And still has the advantage of only one parameter needing to be tuned. PID has lost on the last waypoint (loiter) but that was probably because i set it the wrong way.

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

Now the L1 controller here triumphs. This is a big wind and to cope with it the plane has to fly at a 30º angle. 

Since the PID navigation controller has a limited Integral term it has a slow response, and that gives those bowed curves between waypoints. 

The L1 controller on the other hand follows the waypoints with just a offset (18m), with for photogrammetric applications doesn't matter since what must be preserved is the distance between lines. Another thing to note is how well behaved are the curves between waypoints, having almost always the same radius and a low overshoot. The loiter seams better that what I can do by hand.

I have just one more this to note about the L1 controller in the flight with wind, wich is very meaningful for photogrammetric applications. Here is a plot of the roll angle and longitude  of  the plane while flying the grid part of the mission:

Its easy to the the straight paths between waypoints on the graph ( the linear ramps), what I want to observe is the small roll angle in this pieces (this is when the plane is taking pictures for mapping). Small angles means that a plane without gimbals can be used to take great pictures.

Thanks Brandon for this great code.

[Arthur Benemann]

I forgot to add a link to the log files for my simulations. Since the logs are big I hosted them on Dropbox, and the can be downloaded at this link.

[Andrew Tridgell]

Hi Brandon,

> I didn't test with SITL, only HIL and real flight with an APM1 and Skyfun.
> So let me get SITL up and running to replicate your experience, since it
> doesn't reflect what I saw in HIL and real flight test.

ok, thanks.

Note that if using HIL then make sure you use sensors HIL, not attitude
HIL. The attitude HIL is not for for this sort of testing.

> Was 150 meters larger than the loiter diameter?

yes, I had the loiter radius set to 90m.

> I've also pushed a branch that adds fly-by waypoints, using a turn center,
> entry, and exit waypoint generation function. It uses AP_Mission and
> L1_Control:
> https://github.com/brnjones/ardupilot/commits/Flyby_WP_type

thanks, I'll take a look!

> Concur with possible divide by zero issues, I will address those. This
> also relies heavily on gps ground speed, and would like to leverage the
> ground_speed used in the dead reckoning code you created. Do you see any
> issue there?

yes, we do need this to work with dead-reckoning. We should always be
using the AHRS functions for navigation, for example using
AHRS.airspeed_estimate() instead of the direct airspeed, and use
AHRS.get_position() not the GPS position.

We don't currently have a AHRS.groundspeed_estimate() function, but it
would be easy to add one. It would use GPS ground speed when available,
and use the dead reckoning when not.

In SITL you can test this by setting SIM_GPS_DISABLE to 1, which will
lose GPS lock in the SIM. If you are using the mavproxy map module then
you will see two planes (a red and a blue) for the real and estimated
position of the plane

Cheers, Tridge

[Andrew Tridgell]

Hi Paul,

Thanks for your analysis!

> For this reason I would recommend that reference length be scaled
> automatically with ground speed, and that the user selects a natural
> frequency for the loop.

that sounds very sensible. We often have planes that fly over a wide
range of ground speeds, especially on landing approach vs cruise. To
keep the line-up with the runway on landing good having this scaling
could help a lot.

> I can provide the maths/code for these changes.

yes please!

If you can start with the runtime selectable version that Brandon/Arthur
have done that would be great.

We may also want to move the old algorithm into a library as
well. Eventually if we are happy with the new one we may remove the old
one.

Cheers, Tridge

[Brandon Jones]

Hi Paul,

Thanks for analysis! 

I completely agree with implementing #1 for the reasons you state.  A gain with units of seconds would be very intuitive for people to set.  I also suspect that this gain could be written in terms of the roll-mode time constant of the aircraft, which could be estimated on-line.  But that would be a project on its own.

Also agree the 0.707 damping ratio is a limitation of note.  Definitely interested in your idea on adding additional damping, especially for those airframes that would otherwise require large reference lengths.  Is you idea similar in principle to adding cross-track error-rate feedback?

-Brandon

On Thu, Mar 14, 2013 at 5:27 PM, Paul Riseborough <p_rise...@live.com.au> wrote:

Brandon/Tridge,
 
I've finished looking through Brandons code patch and of the three patches out there at the moment I believe it to be the best solution. Leaving aside the normal implementation cleanup type of stuff (divide by zero protection, etc) there are a couple of algorithm modifications which I would like to propose which will reduce potential tuning difficulties across a wider range of vehicles:
 
1) Looking at the source paper, the natural frequency of the navigaton loop is proportional to the ratio of ground speed to reference length. Because of the potential for coupling between the navigation loop and autopilot bank angle control loop, it is preferable that this frequency be kept constant. For this reason I would recommend that reference length be scaled automatically with ground speed, and that the user selects a natural frequency for the loop. This way a conservative initial value can be set that will work for a wider range of airframes (users can always tweak the requency to a bigger number if they want to tighten up the tracking) and the risk of running into stability issues with higher speed vehicles and tailwinds is reduced.
 
2) The fixed damping ratio of 1/sqrt(2) will limit the natural frequency/gain that can be used before lag from demanded to achieved lateral acceleration eats away the phase margin and damping beccomes unacceptable. Looking at the paper there is simple way to incorporate additional damping using on the n2 angle that will scale automatically with ground speed and reference length. This would enable the user to select the damping ratio for the loop. Unfortunately many of the airframes out there (eg Skywalker) suffer from poor roll rates and adverse yaw, so some adjustment to damping is desirable.

 
I can provide the maths/code for these changes.
 

Regards,
 
Paul
 

> From: tri...@samba.org
> To: brnj...@gmail.com

> Subject: Re: [ardupilot] Plane: L1 Control for Straight and Curved Path Following (#101)

> CC: drones-...@googlegroups.com; p_rise...@live.com.au
> Date: Mon, 11 Mar 2013 10:31:37 +1100

[Brandon Jones]

Hi Arthur,

Thank you very much for adding the parameter, running this code, and providing a detailed write up!  I believe the observed bias could be due to limitations of HIL_MODE_ATTITUDE, since this runs the HIL version of AHRS.  After Tridge pointed this fact out, I ran a similar simulation with HIL_MODE_SENSORS and there is no bias.  It is important to note is the crab angle is not depicted in the mission planner with HIL_MODE_SENSORS, it appears the aircraft icon heading depicts inertial heading rather than yaw angle.

Tridge,

Should I update the wiki to say HIL_MODE_SENSOR does work in Arduplane, or are there still aspects of it that are in work?

-Brandon

[Andrew Tridgell]

> Should I update the wiki to say HIL_MODE_SENSOR does work in Arduplane, or
> are there still aspects of it that are in work?

yes please.

The sensors HIL is very much the preferred HIL mode for ArduPlane and
APMrover2 now. It should also work for ArduCopter, but I haven't tried
it.

Running HIL sensors provides a much better simulation, especially for
things like navigation control. The attitude HIL system can't estimate
wind, whereas the sensors version can.

Cheers, Tridge

[Arthur Benemann]

When I have the time I will redo the simulations with HIL_MODE_SENSOR in X-Plane. 

[Paul Riseborough]

G’Day Brandon,

 

Yes, having a slow roll response is real killer and any adverse yaw will only make it worse as it adds an unstable zero to the transfer function from aileron to turn rate/lat acc. The need to dial out adverse yaw with rudder mixing during the PID tuning process is probably something we should make users aware of.

 

I think I’ve worked out a neat enough way to add the constant frequency  and damping augmentation mods,but wanted to to run them past you before hacking into the code. It’s probably a good time to get some voice comms going if we can work out a suitable overlap between out time zones. I’ll PM you off-line with my details.

 

If you’re looking at the inertial velocity, then once I’ve got the frequency and damping control mods sorted, I’ll start having a look at improving the roll PID which is a source of the noise ‘spike's’ that get get fed through into the aileron servo if the D term is used.

 

I’ve put together a simple roll control architecture which incorporates integration limiting based on control saturation and eliminates the need for a differentiator. This is similar to what I have used in the past, but is missing elements that aren’t appropriate for our application (eg structural dynamics filtering, gain lookup tables, frequency shaping, etc). The attached Simulink model shows the design concept which is similar to what has been proposed previously by Jonathan Challinger, but using body rate rather than flightpath roll rate for feedback and with protection for integrator wind-up.

 

Regards,

 

Paul

 

From: Brandon Jones

Sent: Thursday, March 21, 2013 1:48 PM

To: Paul Riseborough

Cc: tri...@samba.org

Subject: Re: [ardupilot] Plane: L1 Control for Straight and Curved Path Following (#101)

 

Hi Paul!

 

Yes, using the wind corrected bearing in the way I did is not ideal, and getting a reasonable inertial velocity function added to AHRS is required.

 

Last weekend I got the SITL up and running in an ubuntu virtual machine.  I didn't realize, but the Rascal110 modeled in JSBSim is a larger aircraft with a more sluggish roll response and faster ground speed than the X-plane PT-60 or my Skyfun.  This explains the performance Tridge observed, just like you predicted!  I think between your damping term and the fly-by waypoint generation (calculated arc turns) we could get excellent cross track error performance for these types of aircraft.

 

So the short list of items I am working on are:

-Fix a repeatable bug where the aircraft sometimes flies a heading of approx 020 indefinitely (with flyby_WP_type)

-Prototype an inertial velocity implementation.  I bet this is reformulating calculations already performed, and will look at the arducopter code as well.

-Learn how to add a new a mavlink telemetry measurand for debugging/performance data collection.

 

I am new to GIT as well, after searching for a while, I found this book to be the best concise resource: Git - Book,

 

-Brandon

On Wed, Mar 20, 2013 at 3:35 AM, Paul Riseborough <p_rise...@live.com.au> wrote:

Brandon,

 

I’ve spent a bit more time this week looking through your patch trying to work out where to best add the additional damping term and in the process I noticed that in the calc_nu_cd function, you are comparing a wind corrected bearing to the reference point to the yaw angle rather than the true bearing to a velocity vector. I assume this is because you still wish to use the ahrs heading rather than gps velocity heading to enable it to ride out GPs drop-outs?

 

If so then getting inertial velocity added to the AHRS function moves up the priority list because bias errors in wind velocity estimate will still produce bias errors in tracking which reduces one of the main benefits of going to a second order control scheme.

 

Regards,

 

Paul

 

From: Brandon Jones

Sent: Saturday, March 16, 2013 8:15 AM

To: Paul Riseborough

Cc: tri...@samba.org

Subject: Re: [ardupilot] Plane: L1 Control for Straight and Curved Path Following (#101)

 

Hi Paul,

 

Thanks for sharing your design!  I think adding in cross track error rate aspects of this design, as you mentioned in your other message would improve performance for those aircraft that need it.  I am going to work on adding an inertial velocity output in the AHRS library that should help enable this.

 

Overall we are pretty tough on the lateral guidance control laws because after each waypoint sequence there is a large step of cross-track error.  I implemented this path geometry code here first, then found this approach as a way to do curved path following with simple transitions between straight and curved path following.

 

I think one of the most powerful aspects of all this is the ability to prototype flight control laws, it is like we have our own variable stability aircraft!

 

-Brandon

 

On Mon, Mar 11, 2013 at 3:54 AM, Paul Riseborough <p_rise...@live.com.au> wrote:

Brandon,

 

Nice work on the AIAA paper. I like the way it looks after the track capture and following problem in a unified control law and also like the way that for small angles it behaves like a second order system which will effectively removes the reliance on wind estimation for accurate tracking. I have some reservations about the fixed equivalent damping ratio of 1/sqrt(2). My experience ois that phase delays from demanded to achieved lateral acceleration, particularly in a bank to turn airframe, act to reduce the effective damping in the tracking loops and require the damping ratio of the loop to be increased to compensate.

 

To get around this problem in the past, I have used a simple linear controller with gain terms based on position and velocity perpendicular to the demanded trajectory, and set the velocity gain to a larger theoretical damping ratio of around 0.9. This has however required some additional complexity on the form of limiters to control the track capture angle across a range of wind conditions. Is there a way that your scheme can be adjusted to provide some additional damping to compensate for phase loss from demanded to achieved lateral acceleration? – it appears to be an intrinsic property of the control law, but maybe an additional term could be added.

 

I have attached an example Simulink block diagram model showing a simple waypoint to waypoint follower scheme with adjustable damping and the limiter logic required for track capture. The wind estimate is only used to assist with track capture. Note the use of vector computations, with the only trig function required being the atan used to calculate roll angle from acceleration demand. Also note the lag from demanded to achieved acceleration|turn rate in the plant model. This technique can also be applied to curved paths

 

I haven't yet looked at the code itself, but will do so this week.

 

Regards,

 

Paul

 

From: Brandon Jones

Sent: Monday, March 11, 2013 12:49 PM

To: tri...@samba.org

Cc: drones-...@googlegroups.com ; p_rise...@live.com.au

Subject: Re: [ardupilot] Plane: L1 Control for Straight and Curved Path Following (#101)

 

[Brandon Jones]

Paul,

I have written mex files, but only to speed up particular bits of matlab scripts, not to interface with another large program written in C.  Perhaps it is feasible to create a SITL build that would interface with simulink.  The sensor simulation would be the most valuable piece IMHO.

In the short-term, a generic model would be very helpful, perhaps using the same flight dynamics model of the JSBSim Rascal?  There is a JSBSim s-function (http://jsbsim.sourceforge.net/matlab.html), but from what I read, you currently can't trim and linearize with it...

-Brandon

On Thu, Mar 21, 2013 at 3:29 AM, Paul Riseborough <p_rise...@live.com.au> wrote:

Brandon,

 

Yes, I’d like to collaborate on a model development as I find it to be an excellent way to prototype control laws and get all the maths sorted before the complexities of integration start clouding the issues. It is possible to run C and other code within S-function wrappers provided it conforms to the standard interface, but it’s been a while since I’ve used that facility. What version of Simulink do you use?

 

We probably don’t need the complexities of a round earth physics model at this stage (unless we start going down the path of developing tightly integrated INS/GPS filters) so I can knock together a generic airframe model with basic undercarriage dynamics using an earth fixed NED reference frame fairly quickly.

 

Regards,

 

Paul

From: Brandon Jones

Sent: Thursday, March 21, 2013 2:02 PM

To: Paul Riseborough

Cc: Andrew Tridgell

Subject: Re: [ardupilot] Plane: L1 Control for Straight and Curved Path Following (#101)

 

Hi Paul,

 

That is awesome! I will study the Simulink model this weekend. 

 

It would be great if we could collaborate on an Arduplane relevant baseline Simlink model for very rapid control law prototyping, like you've done here.  Perhaps run part of the SITL within a simulink environment?  What are your thoughts?

 

-Brandon

On Mon, Mar 18, 2013 at 12:28 AM, Paul Riseborough <p_rise...@live.com.au> wrote:

G’Day Brandon,

 

I’ve implemented the L1 control law in Simulink and derived and added the additional calculations required to achieve a specified frequency, damping ratio and approach angle. I’ve attached the file for you to have a look at which compares the modified L1 scheme with the simple PD scheme I shared with you earlier. You can se that they essentially give the same results, however the L1 scheme gives explicit user control over approach angle for track capture.The Simulink simulation results indicate that I have the maths right, so I will now look at what is required to get these changes into your code base. I’m new to using GIT, so I haven’t yet worked out how to get the latest files from your L1 patch, other than copying and pasting the text from the github window.

 

I agree with your sentiments on the large step demands we apply to our guidance loops. I’ve used circular arcs in the past to fly past waypoints, but the logic required does get messy after you’ve handled all the special cases(eg acute angle turns, overlapping arcs, etc). At the moment, the user can enter some pretty stupid waypoints (eg 180 deg direction changes), and it won’t fly tidily, but it doesn’t break either.

[Paul Riseborough]

Brandon,
 
I looking at the circular path guidance algorithms, I realised that the need to have the L1 distanceless than or equal to the loiter radius does impose a limitation on the system. Working through the maths, the distance L1 = speed*period/(sqrt(2)*pi), where period is the period of the guidance loop. Typically the period needs to be at least 15 times the roll time constant which equates to about 20 seconds for many airframes. An airframe travelling at 21 m/s (eg 14 m/s airspeed + 7 m/s tailwind) with a 1.5 second roll time constant would therefore have a minimum achievable loiter radius using the proposed scheme of 106 m. Ideally the system should be capable of achieving a minimum loiter radius equivalent to a 45 degree bank turn, which at 21m/s equates to 45 metres .
 
We're going to need to treat loiter as a special case with a modification to the control scheme. I'm in the process of putting together a concept in Simulink for you to have a look at.
 
Regards,
 
Paul

[Arthur Benemann]

About the circular path algorithm:

When the plane is in a distance >> L1_distance the algorithm simple goes set's the nav_cd to the direct heading. Maybe it would be better to use a straight line L1 algorithm until the plane reaches a distance < 2 or 3 times the L1_distance, that way the plane would fly in the line between the last waypoint and the loiter waypoint.