6DOF Attitude controller

[SpaceJockey]

All,
I am writing the code for a compact 6DOF attitude controller for Arduino and request assistance to write an entirely quaternion-based control algorithm which takes an input quaternion from the user, compares this value to the observed quaternion via sensors to generate an error quaternion, and corrects for this error by generating an output quaternion to the actuators. I looked through many sites and read several academic papers on the topic. I understand it is possible, if not better than converting back to Cartesian-space, yet have not found an example of code where the quaternion-based control is implemented. If you agree, I believe it will be most beneficial to make such an Arduino-friendly code readily available to the younger engineers so that they may pursue more exciting projects. 

Has anyone done any work in this area or know of any examples of code similar to the one proposed above?

[jremington]

Most of the open source AHRS programs currently available use quaternions to encode the body orientation, and most are 9DOF. Which 6 do you plan to incorporate?

Here is one example, a second and a third.

[SpaceJockey]

jremington, thank you for the response and diligence. In the control system hierarchy, the 9DOF AHRS provides orientation input in quaternion-space. At this point, I am aware of a variety of control techniques that will operate the plant to achieve a user-specified setpoint (such as PID's), however, I am not familiar with how the control system achieves this when all values are in the form of quaternions.

A simplified one-loop example:
1) read user-established setpoint
2) read AHRS input
3) calculate error between setpoint and input
4) controller adjusts plant output for a known reaction
5) delay by time step and repeat loop

Using the above pseudocode it is possible to achieve stability for a Cartesian system by correcting for error vectors. Will this method likewise work to correct for errors between quaternions?

[jremington]

A problem I see with your line of thinking is that actuators cannot respond to a quaternion.

Actuators work in 3D space (one actuator has a suitably defined 1D space), so you will have to convert the orientation parameters encoded in the quaternion into the actuator spatial system, and that conversion is generally not unique. In fact, in some situations, the conversion cannot be done at all. For example, Euler angles are problematic because of "gimbal lock" (look it up).

Another problem you will face is calculating the error between setpoint and input. One approach would be to define the distance between two quaternions and that is not totally straightforward. Here is some discussion but Google will reveal more.

[SpaceJockey]

Thank you for the link, that was very helpful. You hit the nail on the head for how to mitigate error. From your link: distance*error* = 1-(q1 dot q2)^2

The goal of this control system is to not revert quaternion orientation back to body-fixed actuator space, but to establish all control within quaternion-space. My assumption is that each actuator represents a known quaternion reaction to the body, which will be established in the code setup(). Since the actuator vectors will be constant values, I potentially cut down on the computation required by the Arduino.

Appreciate your input and will get code on the board as soon as possible. Need to find the one Altoid tin that holds my 9 and 10DOF sensors

[jremington]

but to establish all control within quaternion-space

This is by definition non-linear. Look up "inverse kinematics".

[SpaceJockey]

Got it. Continuing the research effort. Thank you for the guidance.

[MarkT]

If the error is at all times kept small it is basically linear if the actuators each apply a torque
proportional to input.

I've used the error quaternion to drive 3 PID loops, basically use the atan2(x, r),
atan2 (y, r), atan2 (z, r) as the output drive levels from error quaternion (r, x, y, z)

With your approach you need to integrate and differentiate the quaternion error -
whather that saves CPU cycles compared to 3 real PID loops is an interesting issue.

With large step changes where the error is close to 180 degrees the error quaternion
is under-determined.  This means rate-limiting step-changes in the input quaternion
is a good idea.

With a quadcopter for instance there are other issues (Z torque much less than XY
torques, saturation of drive levels causes loss of control due to resultant torque
axis locking up).

Interesting project.

[SpaceJockey]

With large step changes where the error is close to 180 degrees the error quaternion
is under-determined.  This means rate-limiting step-changes in the input quaternion
is a good idea.

MarkT,

Do you propose an increased sampling rate to reduce step changes? Appreciate the comment on the variability of torque effects based on configuration. Per jremington's advice, I am testing the waters of inverse kinematics, which I vaguely remember from my academic background. I believe the biggest challenge was the formulation and efficacy of the inertia matrix, which seems a similar challenge to the one you present with the aircraft control axes.

Without a test rig I am figuratively spinning my wheels, since I'd rather do so literally. I will start in the 1-D case and build upon the linear assumption.

[jremington]

Maybe if you told us more about your actual application, we could be of more assistance. I assumed you were thinking of some sort of robot manipulator, with linear actuators.

For quadrotor attitude stabilization mentioned by MarkT above, the math is somewhat simplified and has been worked out in detail. One example that you may have seen is  http://www.nt.ntnu.no/users/skoge/prost/proceedings/ecc-2013/data/papers/0927.pdf

[SpaceJockey]

jremington, understood and you are correct. I fully worked through and verified all math presented in the paper. My task now is to implement this process into the Arduino and get testing! Great find.

The paper identifies that the quaternion-based controller requires "a priori" user-defined inputs into the code based on geometry and component attributes. For this reason, I shall provide a first iteration of code to this forum once complete and shall annotate appropriately for reviewers and potential users.

The goal is to make a computationally efficient controller that is accessible by junior builders and developers to promote interest and participation. This first iteration will be for an aircraft, but may potentially be applied to other robotic systems.

[jremington]

Yes, that paper is very clear. A modular approach to implement these ideas for Arduino would be very useful!

One of the weaknesses with open source AHRS systems is that most developers are constantly adding to the code, in order to support new sensors, airframes, etc. Consequently there tends to be a lot of useless code, with multiple conditional compilation options, that really confuse everyone. I hope you agree that it is a good idea to resist this tendency.

[SpaceJockey]

I agree. The motivation for this effort was to avoid the "black box" solutions that exist. Garbage in = Garbage out. I made significant progress on the pseudocode yesterday, including applicable math and numerical optimizations.

My goal for today is to complete a batch of code for an initial test. To do this efficiently, I shall start by using LED's in place of rotors. I will then watch as the control system attempts to "fight" how I hold the aircraft in my hand relative to a specified orientation. I am looking for the output to affect the correct LED's as well as the controller doing its job and behaving more aggressively if I hold the aircraft at an unwanted angle, such as in a condition with steady cross-breeze. If the LED response is satisfactory, then I can upgrade to rotors and get to tuning the controller parameters.

[SpaceJockey]

UPDATE: I successfully implemented the Madgwick sensor fusion AHRS algorithm in quaternions using an Adafruit 10DOF sensor and Arduino Pro Mini with stable output values. This was great fun while sitting in the coffee shop, but is of no value until I integrate it as a means of closed-loop control. I spent the weekend building portions of the controller test rig so that I may validate the code that I will present. Estimated completion of the test rig should be within one to two weeks.

The biggest challenge I am facing right now is sending floating point values over serial.write() from the user-side microcontroller to the aircraft microcontroller. There are a number of ways to accomplish this according to my research, but all seem to have drawbacks, and get into an area of code writing that I am not as familiar with.

Please submit recommendations and stay tuned for more updates.

[PaulS]

There are a number of ways to accomplish this according to my research, but all seem to have drawbacks

What ways have you tried? What were the drawbacks? A union seems pretty simple, and, since it works, I really can't see any drawbacks.