┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊
长 春 大 学 毕业设计（论文）纸
加速度计测量的对象是比力，也就是等效重力加速度和运动加速度的和，当静止时，运动加速度为0，加速度计的测量值为等效重力加速度，可以利用这一点校正加速度计。加速度计的校正的思路为：对测量值平移和缩放，把测量值拟合到重力加速度。因此校正的任务为：寻找最佳的平移和缩放参数，使总体测量数据的更靠近重力加速度。
记测量值为[ xm ym zm ]T，校正后的值为[ xc yc zc ]T，平移参数为[ ox oy oz ]T，缩放参数为[ gx gy gz ]T。
xc = ( xm + ox )*gx
yc = ( ym + oy )*gy (4-3)
Zc = ( zm + oz )*gz
4.2.数据融合 4.2.1.概述
有了传感器的数据，就可以用来计算姿态了。计算姿态主要用到3个传感器：陀螺仪、加速度计、电子罗盘。加速度计测量对象为比力，受运动加速度影响大，特别是受飞行器机架的振动的影响，振动振幅高达2G。电子罗盘测磁场强度，受环境影响也很大，特别是受电子调速器、电机等大电流器件影响。而陀螺仪则受外部影响弱，稳定性好，但输出量为角速度，需要积分才能得到姿态，无法避免误差的累积。为了得到稳定的、近实时的姿态，对各传感器的数据取长补短，需要研究各种数据融合方法。
4.2.2.姿态插值法
首先介绍自己构思的“姿态插值法”，过程分成两部分：一、如果已知初始姿态，可以利用陀螺积分，不断推算下一个姿态，这部分动态性能好，但误差会累积；二、利用加速度计和罗盘，可以直接算出一个姿态，这个姿态的期望是正确的，但正如上文所述，这个姿态噪声大，不稳定。为了把两个系统的优点结合起来，对两个系统的结果进行插值，得到的值作为当前的姿态。
下面讲“姿态插值法”的计算过程。
首先是第一个部分——陀螺积分算姿态。陀螺的输出为时间离散的角速度，要对时间积分才得到角度。记陀螺输出的角速度为向量g=[ xg yg zg ]T，单位为rad/s，
共 17 页 第 11 页
┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊
长 春 大 学 毕业设计（论文）纸
假定采样间隔足够短，在一个采样时间间隔里，角速度不变，且转角足够小，忽略三角函数高阶项，各轴间相互的影响忽略不计，该间隔里的旋转可以用四元数表示成。
接着讲第二个部分。姿态的定义为两个坐标系统的旋转变换，因此只要知道两对在两个坐标系对应的向量，就可以把姿态求出来。两对变量为加速度和磁场强度，测量出的加速度和磁场强度是在机体坐标系的，而地理坐标系的加速度和磁场是常量，存在一个旋转，可以把机体坐标系的加速度和磁场强度转换到与地理坐标系的对应的常量重合，这个旋转就是所求的姿态。由于传感器系统误差、噪声等影响，测量出的加速度和磁场的夹角不是恒定值，不可能精确旋转到与常量一样，只能求最接近的旋转。最接近的原则可定为：一、旋转后四个向量共面；二、加速度和磁场成一定角度，旋转后的角平分线与常量的角平分线重合。
图4-1 姿态插值法数据流程图
4.2.3.梯度下降法
从实际的使用效果来说，姿态插值法已经满足要求，但用了三角函数，即使经过优化，运算量还是很大，不方便应用到低性能MCU或提高运算频率。Sebastian O.H. Madgwick提出了更加有效的方法——用梯度下降法计算姿态[12]。其思路为：陀螺仪不断积分姿态；同时把加速度和磁场强度的误差表示成姿态的函数，令误差最小的点为极值点，然后用梯度下降法逼近误差最小的点，即纠正了陀螺的积分误差。这个方法，陀螺计算的姿态占主要，加速度计和罗盘只是辅助纠正。
用陀螺积分姿态的方法跟“姿态插值法”的第一部分一样，关键是如何用加速度
共 17 页 第 12 页
┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊
长 春 大 学 毕业设计（论文）纸
和磁场强度纠正误差。首先要定义误差，最直观就是定义为测量值与常量值之差，这样加速度和磁场强度就分别有一个误差了。因为只考虑方向，相减前要规范化。为了减少运算量、利用常量值里的0和1分量，相减之前统一到机体坐标系，而不是地理坐标系。旋转矩阵的逆等于其转置矩阵。
图4-2 梯度下降法数据流程图
4.2.4.互补滤波法
Mark Euston提出了运算量比梯度下降法更小的互补滤波法[13]。相比梯度下降法用加速度和磁场强度计算姿态的梯度，互补滤波法是把加速度和磁场强度的误差构造成纠正旋转，叠加到陀螺测出的角增量上，实现高效的数据融合。
统一到同一坐标系后，规范化的加速度和磁场强度的测量值和常量值可以作一个叉积，叉积的模为角度误差的正弦，小角情况下认为正比于角度，方向根据右手法则，可以作为纠正的旋转轴，乘上一个系数后，可以与陀螺算出的角增量叠加。
共 17 页 第 13 页
┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊
长 春 大 学 毕业设计（论文）纸
图4-3 互补滤波法数据流程图
5.姿态控制
姿态计算出来后，就可以输出控制了。根据被控姿态的表示方式，分为欧拉角控制和四元数控制。控制的思路为：设定一个目标姿态，调整螺旋桨，使测量出的姿态变为目标姿态。为了避免复杂的精确动力学建模，选用PID控制器。
5.1欧拉角控制
由于欧拉角对应3个轴的旋转，当前姿态和目标姿态的差值可以作为控制输入量，角度的误差直接可以对应力矩的输出。如果当前姿态和目标姿态相差不大，可以忽略旋转顺序的影响。
首先把姿态四元数转成欧拉角。欧拉角有多种定义方式，这里定义x轴正方向为航向，绕z轴转的角度为偏航角(yaw)，绕y轴转的角度为俯仰角(pitch)，绕x轴转的角度为滚转角(roll)，旋转顺序为ZYX。具体转换公式见(5-1)。
yaw: ya = arctan( 2wqzq + 2xqyq , 1- 2yq - 2zq )
Pitch: pa = arcsin( 2wqyq - 2zqxq ) (5-1)
Roll: ra = arctan2( 2wqxq + yqzq , 1- 2xq - 2yq )
共 17 页 第 14 页
2
2
2
2
┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊
长 春 大 学 毕业设计（论文）纸
5.2.四元数控制
用欧拉角来控制姿态，每次控制都要算3次三角函数，运算量很大。为了避免三角函数，可以直接用姿态四元数来控制。思路跟欧拉角控制一样，先求姿态差，再把姿态差输入到PID控制器，来输出油门变化量。
6.算法效果
6.1加速度计校正
注：数据在三个轴正负半轴附近采集，各采16组、共96组数据。
6.2姿态计算
为了比较几种姿态计算算法的效果，先在下位机采集数据，然后在电脑上离线处理，这样可以用相同的数据进行计算和比较。数据分两组，分别对应静态和动态的情况，测量时电机都是开的，因此把电机振动也考虑进来了。
先比较静态的情况。因为几种姿态融合方法的思路都是：陀螺为主、加速度计和罗盘用于纠正陀螺误差，因此动态性能取决于陀螺，静态性能取决于加速度计和罗盘，所以静态的情况最能反映姿态融合算法的优劣，三种方法算出的滚转角，取了其中连续的1000个样点，即连续5秒时间的数据。为了公平比较，先把参数调整到临界值，即刚好能纠正陀螺漂移的值。由图可以看出，姿态插值法和互补滤波法效果差不多，
共 17 页 第 15 页