I’d like to implement an LQR control scheme for quadrotors based on the Snapdragon Flight.
The intention behind this is to realise a small agile platform capable of very aggressive manoeuvres.
Overall I would like to contribute the control scheme to the PX4 flight stack so that it can be used by other people.
Therefore I target a clean implementation that goes along with the existing control modules and would like some inputs and hints from the experienced PX4 developer community.
The overall idea is to use the existing EKF2 estimator fed by a vision or motion capture system to get a full state estimate which should include position, attitude, velocity and bodyrates. This part already exists.
The addition is a module that applies an LQR control with a reference point in the form of:
u = u_0 + K * (x - x_0)
u is the control command,
u_0 is the reference input (ex. stable hover point),
K is the gain matrix and
x_0 are the actual and reference state respectively.
The calculation of
x_0 is a solved problem and left out of discussion. However, it should be possible to update
x_0 or only
x_0 for example over MAVLink. The actual state
x should be available from EKF2. State and input convention is also left out of discussion.
The approach so far:
My idea is to implement this as a module and then add a new “experimental” LQR flight mode.
This mode should be exited whenever the state error becomes too large (as a failsafe/failback measure).
If I understand correctly then the
commander module handles the modes, whereas the controllers itself are contained in the
mc_att_control. On activation of the LQR, the two other control modules should therefore stop publishing.
- Test EKF2 with external position measurement from vision or motion capture.
- Implement the messages for reference state and LQR gain matrix.
- Implement the controller itself
- Implement the flight mode used to activate the controller in the
commanderand mixer. (<- this is my biggest concern atm.)
- Implement failsafe measures.
- Is this a meaningful way to implement such an idea?
- Is there any ongoing work similar or related to this?
- Do you (experienced developers) see any obvious flaws in my idea that make it incompatible with the existing structure?
- Is there a chance that such an implementation might find its way into the upstream (master)?
Or does it sound not useful enough for most user?
- Since the
commanderis a rather complex structure, is there a possibility to get some help to implement an additional flight mode in a clean way?
Thanks for your time! I appreciate any advice, hint or comment!