四轴飞行器定速巡航系统设计毕设论文

┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊

长 春 大 学 毕业设计（论文）

和磁场强度纠正误差。首先要定义误差，最直观就是定义为测量值与常量值之差，这样加速度和磁场强度就分别有一个误差了。因为只考虑方向，相减前要规范化。为了减少运算量、利用常量值里的0和1分量，相减之前统一到机体坐标系，而不是地理坐标系。旋转矩阵的逆等于其转置矩阵。

? ? 图4-2 梯度下降法数据流程图

4.2.4.互补滤波法

Mark Euston提出了运算量比梯度下降法更小的互补滤波法[13]。相比梯度下降法用加速度和磁场强度计算姿态的梯度，互补滤波法是把加速度和磁场强度的误差构造成纠正旋转，叠加到陀螺测出的角增量上，实现高效的数据融合。

统一到同一坐标系后，规范化的加速度和磁场强度的测量值和常量值可以作一个叉积，叉积的模为角度误差的正弦，小角情况下认为正比于角度，方向根据右手法则，可以作为纠正的旋转轴，乘上一个系数后，可以与陀螺算出的角增量叠加。

共 20 页 第 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 )

共 20 页 第 14 页

2

2

2

2

┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ 装 ┊ ┊ ┊ ┊ ┊ 订 ┊ ┊ ┊ ┊ ┊ 线 ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊ ┊

长 春 大 学 毕业设计（论文）

5.2.四元数控制

用欧拉角来控制姿态，每次控制都要算3次三角函数，运算量很大。为了避免三角函数，可以直接用姿态四元数来控制。思路跟欧拉角控制一样，先求姿态差，再把姿态差输入到PID控制器，来输出油门变化量。

6.算法效果

6.1加速度计校正

注：数据在三个轴正负半轴附近采集，各采16组、共96组数据。

6.2姿态计算

为了比较几种姿态计算算法的效果，先在下位机采集数据，然后在电脑上离线处理，这样可以用相同的数据进行计算和比较。数据分两组，分别对应静态和动态的情况，测量时电机都是开的，因此把电机振动也考虑进来了。

先比较静态的情况。因为几种姿态融合方法的思路都是：陀螺为主、加速度计和罗盘用于纠正陀螺误差，因此动态性能取决于陀螺，静态性能取决于加速度计和罗盘，所以静态的情况最能反映姿态融合算法的优劣，三种方法算出的滚转角，取了其中连续的1000个样点，即连续5秒时间的数据。为了公平比较，先把参数调整到临界值，即刚好能纠正陀螺漂移的值。由图可以看出，姿态插值法和互补滤波法效果差不多，

共 20 页 第 15 页