6DOF Attitude controller

Projects & Topics Robotics
#3

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?

#4

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.

#5

Thank you for the link, that was very helpful. You hit the nail on the head for how to mitigate error. From your link: distanceerror = 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 :slight_smile:

#6

but to establish all control within quaternion-space

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

#7

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

#8

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.

#9

MarkT: 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.

#10

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

#11

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.

#12

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.

#13

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.

#14

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.

#15

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.

#16

Paul S,

I successfully achieved floating point tx/rx by converting floating point values to integers. This required I adopt a set precision for the data, multiply by this order of magnitude, and add a value equal to the maximum absolute value of the data to remove the +/- signing requirement. I implemented the code that you presented in this discussion: http://forum.arduino.cc/index.php?topic=150896.0 , and likewise performed more math to back out my set-precision floats.

The next challenge is the integration with the control system, specifically, getting a servo to actuate according to the error quaternion. The code is getting verbose at this point, so I hope I may effectively outline the issue to you and receive further guidance:

According to your code, once the "SOP" is read, successive values are stored in an array until the "EOP is read, hence we received a packet. I am able to convert these values to variables while the code is populating the array prior to receipt of "EOP", but am not able to pull values from the array after the complete packet is received. The issue may not stem from this portion of code itself, since this code also works in conjunction with the Adafruit_10DOF library, communicating with the sensor via I2C. Which brings me to my second and potentially related issue and the motivation for this post.........

Although I converted these variables while the array is populating AND successfully generated the error quaternion during stable communication AND validated via Serial.print(the output values to the servo), I get no response from the servo when I implement the servo library's write() command.

Please advise.

#17

I get no response from the servo when I implement the servo library's write() command.

Do you KNOW that the servo works? How is it powered? Lets rule out the stupid mistakes, first.

#18

PaulS, I committed the classic mistake of not having a common ground between power supply and digital pin on the Arduino. Full speed ahead on more detailed experimentation.

Already uncovered one error from real world tests as to be expected. I'm pleased with the way the project is coming along and I hope to deliver the code soon.

#19

I encountered some issues over the weekend that may stem from the quaternion to axis error transformation as described on page 3867 "Controller Synthesis" of the paper provided by jremington,

http://www.nt.ntnu.no/users/skoge/prost/proceedings/ecc-2013/data/papers/0927.pdf

Equation 20 appears to show that the axis error utilized by the control system is simply the vector portion of the error quaternion without performing a transformation to axis-angles. This makes sense because all three vector components are scaled by the same sin(a/2), and is advantageous if possible since it avoids singularities when angular error approaches zero.

As a test of this approach, I performed a 1-D relative rotation about the x-axis, which should intuitively result in an x-axis error only. However, in testing the error is observed on more than one axis, or by an axis other than the x-axis.

The controller presented in the article uses the axis error to control the rotation rates directly, so I am confused as to why I am witnessing this behavior. Any ideas on how to proceed?

#20

Update:

I made a program to test the hypothesis that a 1-D relative rotation should result in a single-axis response and I determined that yes, that is how the control system should respond. However, it is important to note that any rotation involving more than a single axis will result in response by all three axes.

So why did I observe different behavior in my implementation of the system?

Since all control systems operate on the premise of garbage in = garbage out, I revisited the quaternion inputs to my system, obtained via the Adafruit 9DOF sensor in conjunction with the Madgwick sensor fusion algorithm. Rotating my sensor in space, I noticed that for angles of rotation exceeding approximately 45 degrees, there was an axial mixing taking place within the vector portion of the quaternion. Yes they are strange numbers to begin with, and rotations are a confounding problem to visualize, but bear with me.

I performed the same test with quaternions obtained directly from the Bosch BNO055 absolute orientation sensor utilizing the Adafruit BNO055 library and not only were the quaternion values different, but they did not exhibit the axial mixing! Furthermore, the sensor readings were fairly resistant to disturbance.

I went back to Madgwick's Dissertation and apart from being impressed with his efforts, I was unable to find the source of error. All I care about is that I found a solution and can finally progress past this wicked problem.

#21

As you are no doubt aware, rotations do not commute, and there are several different conventions for Euler angles, plus two possibilities for defining a positive rotation.

The choice of angular system must be consistent when composing and decomposing a quaternion into an Euler system, and the data sheet for the BNO055 does not seem to state clearly which convention is used.

Most likely, Madgwick used a different convention for the definition of Euler angles.

#22

BTW is this code available somewhere? It might be fun to have a look sometime :)