温馨提示！由于新浪微博认证机制调整，您的新浪微博帐号绑定已过期，请重新绑定！&&|&&
士为知己者死,女为悦己者容。
LOFTER精选
网易考拉推荐
用微信&&“扫一扫”
将文章分享到朋友圈。
用易信&&“扫一扫”
将文章分享到朋友圈。
而当dt为常值时公式可以简化。从这里我们可以看出，MWC使用的是最原始的PID算法。只是其中D项的0xFFFF我一时并不知为什么会有。
&&&&下面我们就来看下AngleRateTmp是怎么来的：
//----------PID controller----------
for(axis=0;axis&3;axis++) {
//-----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis&2 ) { // MODE relying
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = constrain((rcCommand[axis]&&1) +
GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16
bits is ok here
if (axis == 2) {//YAW is always gyro-controlled (MAG correction is applied to
rcCommand)
&&&&&&AngleRateTmp&=
(((int32_t) (conf.yawRate + 27) * rcCommand[2]) && 5);
if (!f.ANGLE_MODE) {//control is GYRO based (ACRO and HORIZON - direct sticks
control is applied to rate PID
&&&&&&&&AngleRateTmp&=
((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) && 4;
if (f.HORIZON_MODE) {
//mix up angle error to desired AngleRateTmp to add a little auto-level feel
&&&&&&&&&&AngleRateTmp&+=
((int32_t) errorAngle * conf.pid[PIDLEVEL].I8)&&8;
} else {//it's the ANGLE mode - control is angle based, so control loop is needed
&&&&&&&&AngleRateTmp&=
((int32_t) errorAngle * conf.pid[PIDLEVEL].P8)&&4;
这里是控制算法的上半部。
同样，YAW方向是最简单的，将遥控器数据直接算出速率。若是另外两个方向，则需要先获得姿态误差errorAngle。获得姿态误差之后根据不同的模式计算速率。比如ANGLE_MODE模式使用P算法得到速率。而如果是HORIZON_MODE模式则先计算遥控器速率，再对误差积分。而其它模式就直接计算遥控器速率。
那么综合来看，实际上MWC新的控制算法与Crazyflie是一致的，即姿态与速率两个控制器都是串联的方式。我个人也是比较钟情与这种控制方式，主要是控制器的串联很好地解决了姿态与陀螺相互抢的问题。这是之前我自己做四轴很大的一个困扰。
这里所谓附加控制主要值六轴之外的控制。比如地磁，GPS。
那么我们先来说是MWC中的地磁吧。在MWC中地磁控制部分代码并不很多：
if (abs(rcCommand[YAW]) &70 && f.MAG_MODE) {
int16_t dif = att.heading - magH
if (dif &= - 180) dif += 360;
if (dif &= + 180) dif -= 360;
if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.pid[PIDMAG].P8&&5;
} else magHold = att.
这里我们需要知道变量magHold代表的是航向，而att.heading则是当前的方向，那么显然两者的差值就是PID的输入了。同样通过上面的代码我们需要知道这里的单位是度。这个很容易解释，当我们向左转300度的时候实际上与我们向右转60度是一样的，那我们当然会愿意右转60度而不是左转300度。我们知道地磁是用来锁住方向的。即地磁与YAW相关。而MWC的做法是直接将地磁叠加到遥控器数据上。这种做法在MWC中并不少见，比如气压计就直接叠加到油门。
关于航向角att.heading的计算，从MWC2.3的代码中是很难看出来的。那么我们就要追溯历史版本。因为我相信，一个开源项目的升级总是有迹可循的。
&&&&我所使用的MWC源码最早版本是1.9。其中航向角的计算为：
// Attitude of the cross product vector GxM
heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z *
EstM.V.Y - EstG.V.Y * EstM.V.Z& ) / 10;
根据代码即注释，重力与地磁叉积即可得到航向角。为什么？既然是叉积，为什么有偏偏一项没有Y而另一项没有X？而我所能确定的是第一项肯定落在y轴上(向量叉积)，第二项肯定落在x轴上(向量叉积)。而xy轴又在水平面内，这样确实可以得到一个水平面内的夹角。但这个夹角为什么就是航向角我并不清楚。
&&&&暂且我们撇开这些细枝末节，看看航向角的计算都是怎么演变的。
// Attitude of the cross product vector GxM
&&&&heading
= _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y -
EstG.V.Y * EstM.V.Z& ) / 10;
// Attitude of the cross product vector GxM
&&&&heading
= _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y -
EstG.V.Y * EstM.V.Z& );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
if ( heading & 180)&&&&& heading = heading - 360;
else if (heading & -180) heading = heading + 360;
&&&&heading
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
EstM32.V.Y * invG * sqGX_sqGZ& - (EstM32.V.X * EstG32.V.X + EstM32.V.Z *
EstG32.V.Z) * invG * EstG32.V.Y );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
&&&&att.heading
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
(EstM.V.Y * sqGX_sqGZ& - (EstM32.V.X * EstG32.V.X + EstM32.V.Z *
EstG32.V.Z) * EstG.V.Y)*invG );
att.heading += conf.mag_ // Set from GUI
att.heading /= 10;
通过对比各版本我们看到，航向角的计算形式上根本性的改变是从2.2版本开始的。而2.3与2.2只是稍微变换了下形式。并且这种变化主要集中在第二项。但是为什么用invG * sqGX_sqGZ取代EstG.V.Z实在没想明白。只好暂且放下了。
&&&&下面我们先看GPS：
if ( (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME ) {
float sin_yaw_y = sin(att.heading*0.f);
float cos_yaw_x = cos(att.heading*0.f);
#if defined(NAV_SLEW_RATE)&&&&
nav_rated[LON]&& +=
constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT]&& +=
constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL]&& = (nav_rated[LON]*cos_yaw_x -
nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH]& = (nav_rated[LON]*sin_yaw_y +
nav_rated[LAT]*cos_yaw_x) /10;
GPS_angle[ROLL]&& = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH]& = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
GPS_angle[ROLL]& = 0;
GPS_angle[PITCH] = 0;
从前面的控制算法中我们看到GPS对应的有个角度GPS_angle，其实就是我们这里计算得到的。从前面地磁的江苏我们知道0.f是为了将度转弧度的。然后就是旋转。为什么要旋转呢？其实也很好理解。飞机刚开始是直直的飞出去的，机头朝前。但后面转了一个角度。若在此时开启GPS返航，直线返回是最好的。但由于机头已经转向需要把机头转过来，之后按照正常输出就可以了。我们也可以先输出，在对输出进行旋转，但是这样一来计算了势必增大。而其中nav_rated则是平滑处理，这个根据注释可以知道。实际上nav_rated每一次加的都是增量，而增量太大时，由于对增量进行了限制，也就相对平滑。
&&&&关于气压计，MWC中是直接修改油门，这个时候已经不是叠加了。
& #if BARO
&& (!defined(SUPPRESS_BARO_ALTHOLD))
/* Smooth alt change routine , for slow auto and aerophoto modes (in general
solution from alexmos). It's slowly increase/decrease
* altitude proportional to stick movement (+/-100 throttle gives about +/-50 cm&in 1 second with cycle time
about 3-4ms)
if (f.BARO_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
if (abs(rcCommand[THROTTLE]-initialThrottleHold)&ALT_HOLD_THROTTLE_NEUTRAL_ZONE)
// Slowly increase/decrease AltHold proportional to stick movement ( +100
throttle gives ~ +50 cm&in
1 second with cycle time about 3-4ms)
AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleH
if(abs(AltHoldCorr) & 512) {
&&AltHold += AltHoldCorr/512;
AltHoldCorr %= 512;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = alt.EstA
&&isAltHoldChanged = 0;
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
这里并不是气压计的全部代码，从这里我们很容易想到AltHold与BaroPID是有关联的，而initialThrottleHold是某一时刻的油门值。那么变量AltHold是什么呢？根据定义：
从注释的单位我们可以知道，变量AltHold保存的是高度信息，并且单位是cm。alt的类型定义为：
typedef struct {
EstA&&&&&&&&&&&&
&&&&&&&&&&&&&
// variometer in cm/s
该结构有两个成员，一个是高度(cm)，一个是速度(cm/s)。而这个速度显然指的是上升或是下降的速度。在阅读这段代码的时候，我经常犯的一个错是忽略气压计是用户选择开启的，而不是飞行过程中一直存在。这样就导致我想不通高度是怎么下降的。那么我们需要简单了解下MWC中的气压计模式。气压计模式开启的时候，会记录当前的油门：
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
&&&&&&&&&&&
f.BARO_MODE = 1;
&&&&&&&&AltHold = alt.EstA
&&&&&&&&&&&
#if defined(ALT_HOLD_THROTTLE_MIDPOINT)
&&&&&&&&&&&&&
initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;
&&&&&&&&&&&
&&&&&&&&&&&&&&initialThrottleHold
= rcCommand[THROTTLE];
&&&&&&&&&&&
&&&&&&&&&&&
errorAltitudeI = 0;
&&&&&&&&&&&
BaroPID=0;
f.BARO_MODE = 0;
这样就产生了我们看到的initialThrottleHold。同时对AltHold进行了初始化，也就是锁在当前高度。如果我们需要改变高度，操作过程是改变油门直到与锁定油门的差值超过一个阈值(ALT_HOLD_THROTTLE_NEUTRAL_ZONE)。当到大预定高度只需将油门打回锁定油门(initialThrottleHold)即可。这样我们再来看最开始的那段代码就很好理解了。当油门差值没有超过阈值时锁定当前高度。当油门差值超过阈值时使用简单算法缓慢改变高度。
&&&&到这里，我们还有一个东西需要搞明白：BaroPID。显然这是气压计PID算法输出：
getEstimatedAltitude(){
int32_t& BaroA
float baroGroundTemperatureScale,logBaroGroundPressureS
float vel =&0.0f;
uint16_t previousT;
& uint16_t
currentT = micros();
& uint16_t
currentT - previousT;
& if (dTime
& UPDATE_INTERVAL) return 0;
& previousT
= currentT;
if(calibratingB & 0) {
logBaroGroundPressureSum = log(baroPressureSum);
baroGroundTemperatureScale = (baroTemperature + 27315) *&&29.271267f;
calibratingB--;
& // baroGroundPressureSum
is not supposed to be 0 here
https://co/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
& BaroAlt =
( logBaroGroundPressureSum - log(baroPressureSum) ) *
baroGroundTemperatureS
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) && 3; // additional LPF to
reduce baro noise (faster by 30&碌s)
(defined(VARIOMETER) && (VARIOMETER != 2)) ||
!defined(SUPPRESS_BARO_ALTHOLD)
int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);
applyDeadband(error16, 10); //remove small P parametr to reduce noise near
zero position
&&&&BaroPID
= constrain((conf.pid[PIDALT].P8 * error16 &&7), -150, +150);
errorAltitudeI += conf.pid[PIDALT].I8 * error16 &&6;
errorAltitudeI = constrain(errorAltitudeI,-);
&&&&BaroPID
+= errorAltitudeI&&9; //I in range +/-60
// projection of ACC vector to global Z, with&1G&subtructed
// Math: accZ = A * G / |G| -&1G
int16_t accZ = (imu.accSmooth[ROLL] * EstG32.V.X + imu.accSmooth[PITCH] *
EstG32.V.Y + imu.accSmooth[YAW] * EstG32.V.Z) * invG;
static int16_t accZoffset = 0;
if (!f.ARMED) {
accZoffset -= accZoffset&&3;
accZoffset += accZ;
accZ -= accZoffset&&3;
applyDeadband(accZ, ACC_Z_DEADBAND);
static int32_t lastBaroA
//int16_t baroVel = (alt.EstAlt - lastBaroAlt) *&f&/ dT
int16_t baroVel = (alt.EstAlt - lastBaroAlt) * (1000000 / UPDATE_INTERVAL);
lastBaroAlt = alt.EstA
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/-&300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
// Integrator - velocity, cm/sec
vel += accZ * ACC_VelScale * dT
// apply Complimentary Filter to keep the calculated velocity based on baro
velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ
(velocity) without loosing the phase, i.e without delay
vel = vel *&0.985f&+
baroVel *&0.015f;
alt.vario =
applyDeadband(alt.vario, 5);
&&&&BaroPID
-= constrain(conf.pid[PIDALT].D8 * alt.vario &&4, -150, 150);
& return 1;
其中P与I项很容易看出来。而D项虽然比较绕，但显然是没有错的，对高度微分自然就是上升(或下降)的速度。计算速度的时候同样使用了线性插值。MWC中线性插值用的比较多，有时候我都会误以为是均值滤波。但是线性插值中的这两个值vel与baroVel，其中vel却比较绕。我们先说baroVel吧，高度差对时间微分得到的肯定就是速度了。而vel就不同了，一开始我想不明白vel是怎么与重力扯上关系的，知道后来一句注释提醒了我：
// Math: accZ =
A * G / |G| -&1G
什么意思呢？简单点说就是超重(或者失重)。但凡物体在竖直方向上做变速运动势必会超重(或者失重)，而超重(或者失重)量自然就是物体的加速度。从物理学中我们知道速度的微分是加速度，vel就是这么来的。上面的注释实际上是将加计投影到重力方向，然后做差。但程序中这是分两步完成的，先是投影，然后做差。代码分别如下：
int16_t accZ =
(imu.accSmooth[ROLL] * EstG32.V.X + imu.accSmooth[PITCH] * EstG32.V.Y +
imu.accSmooth[YAW] * EstG32.V.Z) * invG;
accZoffset&&3;
刚开始我一直以为下面那条语句是为了去除误差，就像陀螺处理漂移一样。知道后来发现上面那条语句是投影我才明白过来。再结合代码稍作分析就确定了。这样我们再来看速度增量的计算：
vel += accZ *
ACC_VelScale * dT
ACC_VelScale (9.80665f&/&10000.0f&/ ACC_1G)
我们知道，速度的增量是加速度对时间的积分，那显然我们需要对公式进行整理变形。除此之外我们还需要了解四点信息：首先这里计算得到的速度增量的单位是cm/s；二，加速度换算出来单位是1G而不是m/s^2；三，accZ没有将单位转成1G；四，dTime的单位是us。综合以上四条信息，速度增量的计算应为：
v = (accZ/1G)*g * (dTime/10^6)*100;&&à
v = accZ*(g /(1G*10^4)) * dT
/(1G*10^4))便是这里的宏ACC_VelScale。只是这里的ACC_1G我实在觉得不对，其定义为：
根据我的了解，MPU6050为16位AD，MWC设置的测量范围是+/-8G，那么1G大小也应该是4096，而不是512.那是否是数据那某个地方被除了个8，而我没发现？如果是这样，那么查下代码就应该能找到。于是我找到了加计的读函数：
void ACC_getADC
& i2c_getSixRawADC(MPU6050_ADDRESS, 0x3B);
ACC_ORIENTATION(&((rawADC[0]&&8) | rawADC[1])&&3 ,
&&&&&&&&&&&&&&&&&&
((rawADC[2]&&8) | rawADC[3])&&3 ,
&&&&&&&&&&&&&&&&&&
((rawADC[4]&&8) | rawADC[5])&&3 );
ACC_Common();
几次我看到这个函数我都看错了，我都以为是第八位右移了三位，知道我查程序里边丢失的这个8的时候，我才发现这里原来是将结果右移了三位即除8。
&&&&那么到这里，所有的分析与推测跟实际代码就都是吻合的了。以上就是MWC2.3版本中的气压计定高，内容比较多，也很繁杂。不管代码怎么千变万化，理清了，其实也不复杂。
做过四轴的肯定还有这样的印象：当四轴在空中基本悬停的时候，如果打摇杆使四轴水平移动高度势必会降。为什么呢？说白了就是升力被分了一部分出去用来提供水平方向的动力，由于没有补油门，重力方向上肯定没办法抵销重力，高度因此降低。所以我们这里要说的就是MWC如何处理这个问题。
defined(THROTTLE_ANGLE_CORRECTION)
if(f.ANGLE_MODE || f.HORIZON_MODE) {
rcCommand[THROTTLE]+= throttleAngleC
代码不多，关键是throttleAngleCorrection的数据是怎么来的：
defined(THROTTLE_ANGLE_CORRECTION)
cosZ = EstG.V.Z / ACC_1G&*&100.0f;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
// cos(angleZ) * 100
throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ,
0, 100) &&3;& // 16 bit ok: 200*150 = 30000&
以前我总是想办法通过xy轴来补油门，效果总是不理想。而MWC的做法然我很意外，也比我的做法巧妙得多。因为不论x轴变了或是y轴变了，z轴肯定也要跟着变。
&&&&尽管这些代码我反复阅读，但写文档过程中发现人有许多不解之处。这些疑惑在前面文档中用红色粗体标识，现罗列如下，希望能在后面阅读代码过程中解决。
对P项进行约束
&&&&这是2.2MWC控制中的一个困惑。对输出进行约束原本很正常，问题在于这里是用D项系数对P项输出进行约束，而且把P项输出限制得比较小。
只是这里使用D8对P项进行约束却有些不理解，毕竟当D8为0时，P项的极限值是300，未免也太小了点。
陀螺的重复使用
&&&&这依然在2.2MWC控制中。MWC中老的控制算法中陀螺重复引入控制，并且第二次引入使用了动态调参。当然更重要的还是在于这样破坏了PID算法的结构。因为动态调参并没有引入原本的PID算法当中。有或者这是MWC中姿态控制的第三个控制器：陀螺P控制？就我个人目前的理解而言这样自会加剧陀螺与姿态之间的竞争。
特别是P项，对error做了P运算之后又引入了一次陀螺量。而且这一次仅仅对P项引入了。
&&&&这里的困惑是在2.3 MWC控制控制算法2当中，
只是其中D项的0xFFFF我一时并不知为什么会有。
标志PID算法为：
二人MWC中代码为：
((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime&&4)))&&6;
即使在数学上对其变形：
delta = (delta /
cycleTime)* (0xFFFF/4);
对于最后一项的意义依然不是很明了。
阅读(1802)|
用微信&&“扫一扫”
将文章分享到朋友圈。
用易信&&“扫一扫”
将文章分享到朋友圈。
历史上的今天
在LOFTER的更多文章
loftPermalink:'',
id:'fks_',
blogTitle:'四轴飞行器控制算法_2',
blogAbstract:'& & & &&之前在博客中发了几篇关于四轴飞行器的文档，有些人觉得对自己有些帮助，于是发邮件给我说怎么没有更新了。其实是后面都没什么时间去整理文档，这片是匆忙之中找到的之前写的没有上传的，至少当下也没有时间去推敲里面的每一个东西，若是有所纰漏还望指出。\n\n',
blogTag:'crazyflie,mwc,四轴飞行器',
blogUrl:'blog/static/',
isPublished:1,
istop:false,
modifyTime:2,
publishTime:8,
permalink:'blog/static/',
commentCount:10,
mainCommentCount:2,
recommendCount:0,
bsrk:-100,
publisherId:0,
recomBlogHome:false,
currentRecomBlog:false,
attachmentsFileIds:[],
groupInfo:{},
friendstatus:'none',
followstatus:'unFollow',
pubSucc:'',
visitorProvince:'',
visitorCity:'',
visitorNewUser:false,
postAddInfo:{},
mset:'000',
remindgoodnightblog:false,
isBlackVisitor:false,
isShowYodaoAd:false,
hostIntro:'士为知己者死,女为悦己者容。',
hmcon:'1',
selfRecomBlogCount:'0',
lofter_single:''
{list a as x}
{if x.moveFrom=='wap'}
{elseif x.moveFrom=='iphone'}
{elseif x.moveFrom=='android'}
{elseif x.moveFrom=='mobile'}
${a.selfIntro|escape}{if great260}${suplement}{/if}
{list a as x}
推荐过这篇日志的人：
{list a as x}
{if !!b&&b.length>0}
他们还推荐了：
{list b as y}
转载记录：
{list d as x}
{list a as x}
{list a as x}
{list a as x}
{list a as x}
{if x_index>4}{break}{/if}
${fn2(x.publishTime,'yyyy-MM-dd HH:mm:ss')}
{list a as x}
{if !!(blogDetail.preBlogPermalink)}
{if !!(blogDetail.nextBlogPermalink)}
{list a as x}
{if defined('newslist')&&newslist.length>0}
{list newslist as x}
{if x_index>7}{break}{/if}
{list a as x}
{var first_option =}
{list x.voteDetailList as voteToOption}
{if voteToOption==1}
{if first_option==false},{/if}&&“${b[voteToOption_index]}”&&
{if (x.role!="-1") },“我是${c[x.role]}”&&{/if}
&&&&&&&&${fn1(x.voteTime)}
{if x.userName==''}{/if}
网易公司版权所有&&
{list x.l as y}
{if defined('wl')}
{list wl as x}{/list}四轴飞行器姿态算法 - 为程序员服务
四轴飞行器姿态算法
最近四周飞行器比较火，自己也买了一个开源的四周飞行器来玩，花了点时间来学习飞行器的空中姿态控制原理。觉得这小东西不简单，并且原理很有趣, 最有趣的部分就是空中姿态的解算。也是无人机控制部分的精华之一。
姿态解算(attitude algorithm),是指把陀螺仪，加速度计, 罗盘等的数据融合在一起，得出飞行器的空中姿态，飞行器从陀螺仪器的三轴角速度通过四元数法得到俯仰，航偏，滚转角，这是快速解算，结合三轴地磁和三周加速度得到漂移补偿和深度解算。
姿态的数学模型
姿态解算需要解决的是四轴飞行器和地球的相对姿态问题。地理坐标系是固定不变的，正北，正东，正上构成了坐标系的X，Y，Z轴用坐标系R表示，飞行器上固定一个坐标系用r表示，那么我们就可以适用欧拉角，四元数等来描述r和R的角位置关系。
姿态的数学表示
姿态有多种数学表示方式，常见的是四元数，欧拉角，矩阵和轴角。在四轴飞行器中使用到了四元数和欧拉角,姿态解算的核心在于旋转。姿态解算中使用四元数来保存飞行器的姿态，包括旋转和方位。在获得四元数之后，会将其转化为欧拉角，然后输入到姿态控制算法中。姿态控制算法的输入参数必须要是欧拉角。AD值是指MPU6050的陀螺仪和加速度值，3个维度的陀螺仪值和3个维度的加速度值，每个值为16位精度。AD值必须先转化为四元数，然后通过四元数转化为欧拉角。在四轴上控制流程如下图：
下面是用四元数表示飞行姿态的数学公式，从MPU6050中采集的数据经过下面的公式计算就可以转换成欧拉角，传给姿态PID控制器中进行姿态控制.
PID控制算法
先简单说明下四轴飞行器是如何飞行的,四轴飞行器的螺旋桨与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴就可以起飞了。四轴飞行器飞行过程中如何保持水平：我们先假设一种理想状况：四个电机的转速是完全相同的是不是我们控制四轴飞行器的四个电机保持同样的转速，当转速超过一个临界点时（升力刚好抵消重力）四轴就可以平稳的飞起来了呢？答案是否定的，由于四个电机转向相同，四轴会发生旋转。我们控制四轴电机1和电机3同向，电机2电机4反向，刚好抵消反扭矩，巧妙的实现了平衡, 但是实际上由于电机和螺旋浆本身的差异，造成我们无法做到四个电机产生相同的升力，这样飞行器起飞之后就会失去平衡。
PID控制器通过PID三个参数来对被控对象进行控制，是应用最广泛一种的控制器
比例(P)控制器
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差。
积分(I)控制器
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的 或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积 分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。
微分(D)控制器
微分调节就是偏差值的变化率。使用微分环节能够实现系统的超前控制。如果输入偏差值线性变化，则在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间。因为只有时间滞后的系统才需要附加这个参数。如果画蛇添足加上这个参数反而会使系统的控制受到影响。
四轴飞行器中的控制器
目前四轴飞行器中所使用的是增量事PD控制器，下面以ROLL方向为例
测得ROLL轴向偏差
偏差 = 目标期望角度 – 传港器实测角度
DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.R
比例项计算
比例项输出 = 比例系数P * 偏差
Proportion = PID_Motor.P * DIF_ANGLE.x;
微分输出 = 微分系数 × 角速度
ROLL方向总控制量 = 比例项输出+微分量输出
Motor[2] = (int16_t)(Thr - Pitch - Roll - Yaw );
Motor[0] = (int16_t)(Thr + Pitch + Roll - Yaw );
Motor[3] = (int16_t)(Thr - Pitch + Roll + Yaw );
Motor[1] = (int16_t)(Thr + Pitch - Roll + Yaw );
Roll方向旋转，则电机1电机2同侧出力，电机0电机3反向出力
Pitch方向旋转，则电机2电机3同侧出力，电机0电机1反向出力
Yaw方向旋转，则电机1电机3同侧出力，电机0电机2反向出力
走别人没有走过的路，让别人有路可走
原文地址：, 感谢原作者分享。
您可能感兴趣的代码}