Using "DCM Lite" for full 6DOF estimation and control of a quadrotor?
All (but perhaps Bill P and Paul B most of all :-),
Based on earlier threads, I had the following idea for the orientation estimation of a quadrotor using only 3 gyroscopes and 3 accelerometers. I'm not sure if it would work.
I propose to track the "gravity vector". I'll update the vector as per the usual DCM algorithm (iterate the vector each timeframe via the cross product of omega and the vector, where omega is the gyro output compensated via a PI feedback of the cross product of itself and the accelerometer outputs). This effectively gives two of the three orientation components, and has already been described elsewhere as "DCM lite". However, I would only update the PI loop when the magnitude of the gravity vector is sufficiently close to 1 g.
The third component comes from integrating the dot product of the compensated gyro signals with the gravity vector - i.e. the component of angular velocity that is around the gravity vector. I'm pretty confident that this gives me a uniquely defined orientation, without singularities (assuming I restrict the angle to +/- 180 deg or equivalent). This scheme seems analogous to the "axis-angle" formulation or even the quaternion formulation in that 4 parameters are used to specify the 3 degrees of freedom (3 components of a vector and an angle).
My experience is that the MEMS gyro drift is pretty small and slowly varying, so that as long as the quad is maneuvering (tilting), all 3 gyros will have some compensation from the PI feedback of the accelerometer error, even without magnetometer feedback (it won't be perfect, but it might be good enough for the 15-30 minutes that the battery will last).
Does this sound like it might work? If so, how would I transform the 4 parameters into a representation of orientation in the inertial frame (DCM, Euler angles, etc) for use in groundstation displays? My idea was to take the cross product of the gravity vector with the body z axis and reverse it to indicate how the body moved, then simply rotate the inertial body around the gravity vector by the integrated angle. However, this did not appear to work in my simulation. Perhaps my code had an error, or perhaps there is something wrong with these ideas.
As for control, my idea is that the pitch & roll r/c joystick inputs would serve as virtual angular velocity commands to move a "desired" gravity vector (for a rate command system). Then the cross product between the desired and estimated gravity vector can be used to form the error signal that drives the PID loops. I could have the yaw joystick command the rate about the gravity vector, but I think it would be more intuitive for it to command the body axis yaw rate (and easier to implement)
Thanks
Roy
Based on earlier threads, I had the following idea for the orientation estimation of a quadrotor using only 3 gyroscopes and 3 accelerometers. I'm not sure if it would work.
I propose to track the "gravity vector". I'll update the vector as per the usual DCM algorithm (iterate the vector each timeframe via the cross product of omega and the vector, where omega is the gyro output compensated via a PI feedback of the cross product of itself and the accelerometer outputs). This effectively gives two of the three orientation components, and has already been described elsewhere as "DCM lite". However, I would only update the PI loop when the magnitude of the gravity vector is sufficiently close to 1 g.
The third component comes from integrating the dot product of the compensated gyro signals with the gravity vector - i.e. the component of angular velocity that is around the gravity vector. I'm pretty confident that this gives me a uniquely defined orientation, without singularities (assuming I restrict the angle to +/- 180 deg or equivalent). This scheme seems analogous to the "axis-angle" formulation or even the quaternion formulation in that 4 parameters are used to specify the 3 degrees of freedom (3 components of a vector and an angle).
My experience is that the MEMS gyro drift is pretty small and slowly varying, so that as long as the quad is maneuvering (tilting), all 3 gyros will have some compensation from the PI feedback of the accelerometer error, even without magnetometer feedback (it won't be perfect, but it might be good enough for the 15-30 minutes that the battery will last).
Does this sound like it might work? If so, how would I transform the 4 parameters into a representation of orientation in the inertial frame (DCM, Euler angles, etc) for use in groundstation displays? My idea was to take the cross product of the gravity vector with the body z axis and reverse it to indicate how the body moved, then simply rotate the inertial body around the gravity vector by the integrated angle. However, this did not appear to work in my simulation. Perhaps my code had an error, or perhaps there is something wrong with these ideas.
As for control, my idea is that the pitch & roll r/c joystick inputs would serve as virtual angular velocity commands to move a "desired" gravity vector (for a rate command system). Then the cross product between the desired and estimated gravity vector can be used to form the error signal that drives the PID loops. I could have the yaw joystick command the rate about the gravity vector, but I think it would be more intuitive for it to command the body axis yaw rate (and easier to implement)
Thanks
Roy
Replies to This Discussion
-
DeveloperPermalink Reply by William Premerlani on -
Roy,
It sounds to me that your idea might work, but only if you limit to manuevers that do not involve much centrifugal effects.
With regard to the gyro yaw drift in the earth frame, the more the quadrocopter tilts back and forth, the better will be the reduction in the drift. Someone (was it you?) asked me about this recently, so I ran my roll-pitch-yaw demo, where I intentionally created a significant amount of initial yaw rate error by yawing the board during power up. At first, the representation was "dizzy", but by rolling and pitching the board, I was able to all but cancel it out. Of course, if you use the controls to keep the quadrocopter level, then the effect will not be there to help. In any case, you will not be able to achieve "lock", of course, but the residual drift rate should be only a few degrees a minute, so what you will have will be a "heading hold".
With regard to integrating the dot product of the gyro signals with the gravity vector, of course you do that by taking the dot product of the gyro vector with the bottom row of the direction cosine matrix (DCM Lite), I think that is what you meant when you referred to the "gravity vector". The integration will give you the yaw angle.
With regard to combining everything back into a single representation, there are two approaches that you could use. If you are careful, either one should work.
A. Work with Euler angles and DCM lite. In this approach, you already have the yaw angle by integrating the dot product. You get the roll and pitch angles from appropriate operations of the elements of the bottom row of the direction cosine matrix. So, then you have all three Euler angles. If you want, you can then transform them into direction cosines, using the standard equations.
B. Work with direction cosines directly. This is a bit of a step away from DCM lite. In this approach, you simply run the full DCM algorithm, you simply do not perform yaw drift compensation. Essentially, you will be doing what I tried recently with my roll-pitch-yaw demo without a GPS for yaw compensation.
But, back to centrifugal effects....without centrifugal compensation, certain manuevers will cause problems. Hovering will be ok, and straight line flying will be ok, and gentle turns will be ok. But if you tried to roll the quadrocopter a bit, and fly it around a banked turn, eventually the centrifugal effect would cause trouble.
Best regards,
Bill
-
Permalink Reply by Roy Brewer on -
Bill,
Thanks for replying.
I would only use the accelerometer vector when its magnitude was about 1 g, so I don't think centrifugal forces will be an issue since, by definition, the acceleration would be larger than 1 g during this maneuver (for constant altitude turns, anyway). Yes, the quad would be subject to gyro drift in this case, but at least the feedback wouldn't be corrupting the attitude estimate. That's sort of the main premise - that the gyro drift is small and slow and therefore we can afford to let it drift for (short) periods of time without incurring too much error. The quad shouldn't need to perform these maneuvers very often, anyway (unlike a fixed wing plane).
At first, I didn't think the Integrated angle that I was referring to was the same as the Euler yaw angle, but now I see that it is. That helps! I was confusing myself between the body axis and inertial axis systems. It should be a lot easier to do the conversion now.
- Roy
-
Permalink Reply by XING on
-
Hi, I'm currently using UAV v2 Development Platform to control a quadrotor UAV. Based on the open source (rmat.c) given, there are some programming part that I dont understand with:
#define GGAIN SCALEGYRO*6*(RMAX*0.025) // integration multiplier for gyros 15mv/degree/sec
fractional ggain = GGAIN ;
#define GYROSAT 15000
Can somebody explain to me how to get GGAIN (what does SCALEGYRO*6*(RMAX*0.025) mean?) and why the GYROSAT is set to be 15000?
Thank you and regards,
Xing
-
DeveloperPermalink Reply by William Premerlani on -
Hi Xing,
You can ignore GYROSAT, it is left over from the earlier demos, and I forgot to take it out.
Regarding GGAIN, it is the scale factor used to convert the raw binary output of the gyros into the values used to perform the integration of the kinematic equations of rotation. It depends on the A/D reference voltage and the sensitivity of the gyros. The earlies version of the UAV DevBoard had gyros with 15 mv/degree/sec, with an A/D reference of 5 volts, so SCALEGYRO was 1.0. RMAX represents the equivalent of 1.0 in the integer number system that we use. 0.025 is the time step. The value 6 is an approximation for the product of all the various factors that impact the gain.
So, basically what you need to do is to convert the raw A/D values that you are getting into radians per second multiplied by the time step.
For example, if you were using a representation where you have converted the gyro signals into units of radians/second, then GGAIN would be equal to the time step in seconds.
Best regards,
Bill
-
Permalink Reply by XING on
-
Hi Bill,
Thank you for your fast response and clear explanation. There is one thing in the explanation that I'm not really understand with. You said 'the earlies version of the UAV DevBoard had gyros with 15 mv/degree/sec, with an A/D reference of 5 volts, so SCALEGYRO was 1.0'.
My question is do I have any mathematical relation between the sensitivity and A/D reference voltage in order to get the SCALEGYRO? Besides, I'm not sure whether I'm referring to the same source code that you are explaining about. It is because the SCALEGYRO that defined in the source code is 3.0 instead of 1.0.
Thank you and regards,
Xing
-
DeveloperPermalink Reply by William Premerlani on -
Hi Xing,
The first version of the board, the so-called green board, had gyros with a 75 degree/second range and with 15 mv/degree/second sensitivity, and a 5 volt supply.
The present version of the board, the so-called red board, has gyros with a 300 degree/second range and with 3.3 mv/degree/second sensitivity, and a 3.3 volt supply.
So, if you work the numbers out, the red-board gyros produce binary values that are 1/3 the values of the green board, for the same rotation rate. Therefore, we have to multiply them by 3 to make them equivalent to the green-board values.
When the red board became available, I added SCALEGYRO to the code to take care of both versions of the board. Setting SCALEGYRO =1 is for the original, green board gyros.
Setting SCALEGYRO=3 takes care of the gyros on the red board.
Best regards,
Bill
-
Permalink Reply by XING on
-
Hi Bill,
Thank you for your explanation by comparing the green and red board. From the RollPitchYaw source code, I have tried to manipulating the values in the code. In the session of updating gyro outputs become 'theta', the code is multiplying the omegaAccum with the ggain. The question is if I take out the RMAX in ggain, the update process seem to be failed to work. Can I know what is the reason behind? (GGAIN= SCALEGYRO*6*RMAX*0.025)
Second, according to GPSUAV2MANUAL_v29, the analog signals from gyros and accelerometer are read on the following A/D channels :
XRATE @ AN2/RB2
YRATE @ AN3/RB3
ZRATE @ AN1/RB1
3.3Volts @ AN0/RBO
XACCEL @ AN6/RB6
YACCEL @ AN7/RB7
ZACCEL @ AN7/RB7
But is the source code, these A/D values are stored in ADCBUF1, ADCBUF2, ... ADCBUF6 and ADCBUFF. My question is why the A/D results for accelerometer are stored in ADCBUF4--->ADCBUF6 instead of ADCBUF6--->ADCBUF8?
Thank you very much.
Best regards,
Xing
-
DeveloperPermalink Reply by William Premerlani on -
Hi Xing,
The reason for RMAX is that all of the calculations are done in integer arithmetic with an implied binary point. RMAX is the integer representation of 1.0, it is equal to 2**14, or 16384. We use integer arithmetic because it is more efficient than floating point, about 50 times more efficient. (I base that estimate on the fact that the DCM calculation in MatrixPilot takes about 1% of the CPU power, while others who have implemented DCM in floating point report about 50% CPU loading.)
Regarding the numbering of the A/D buffers, they do not have anything to do with the numbering of the channels, they simply have to do with the sequencing of the samples. As samples are taken, they are stored sequentially in the A/D buffer slots.
Best regards,
Bill
-
DeveloperPermalink Reply by William Premerlani on -
Hi Xing,
One more thing about the A/D pins and buffers.
When you set up the A/D, you specify which A/D pins to read. The A/D skips over the unused pins.
Best regards,
Bill
-
Permalink Reply by XING on
-
Hi Bill,
Thanks again for your explanation.
I would like to ask if it is possible to use both accelerometer and gyroscopes to obtain the orientation information like what it is shown in this website: http://www.starlino.com/imu_guide.html.
The summary of that website is to obtain the accelerometer and gyros outputs. Then estimate the current output by using a weighting factor before updating the PWM.
In the source code given, you are using the gyro values to update the PWM in addition to compute the PI controller with the help from accelerometer and gps values. For my case, I didn't use the GPS module and would like to use only gyros and accelerometer. If I use only gyros output to update the PWM, it works; but failed to update PWM using accelerometer outputs.
Can I know is the way that I used (based on starlino website) is workable?
It will be the best if you could provide me explanation with sample codes.
Thank you very much.
Regards,
Xing -
-
Permalink Reply by skystalker on
-
HI XING:
I think its important to know about how to calcuate the "6" in "#define GGAIN SCALEGYRO*6*RMAX*0.025) ".
Supouse the AD value readed from the buffer is A (-0.5=<0.5),and the gyro output voltage V (0-5Volt).The relation betwe...
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The third round was a reliablilty/aerial photography round for both planes and copters, which is now closed. Stay tuned for the next round, beginning soon.A list of all T3 contests is here.
Top Content
Groups
-
ArduPilot (legacy) User …
94 members
-
Moderators
57 members
-
DIYDrones - CNC Project
60 members
-
ArduCopter (traditional …
115 members
-
UK Ardupilot Group
29 members
© 2012 Created by Chris Anderson.
