无人机传感器基础课件.pptx

《无人机传感器基础课件.pptx》由会员分享，可在线阅读，更多相关《无人机传感器基础课件.pptx（66页珍藏版）》请在三一办公上搜索。

1、Drone Technology Seminar In ShangHai,无人机技术培训,01,无人机中的传感器,超声波传感器气压计GPS全球定位RTK高精度定位射频传感器组摄像头模组面阵雷达红外等感光传感器,惯性传感器： 陀螺仪 加速度计 磁力计,摄像头模组感光扫描器件,2.4G通信5G通信433通信GPRS、4G模块语音识别模块蜂鸣器、LED等,无人机传感器组成,惯性传感器,MEMS即微电子机械系统。它可将机械构件、光学系统、驱动部件、电控系统集成为一个整体单元的微型系统。这种微电子机械系统不仅能够采集、处理与发送信息或指令，还能够按照所获取的信息自主地或根据外部的指令采取行动。 惯性传感

2、器主要是检测和测量加速度、倾斜、冲击、振动、旋转和多自由度(DoF)运动，是解决导航、定向和运动载体控制的重要部件。 无人机中常见的MEMS惯性传感器有： 加速度计、陀螺仪、磁力计等，其中所有的数据都是沿着X、Y、Z三个轴向检测而来。, 陀螺仪测得载体坐标系下的角速度数据,加速度计测得载体坐标系下重力加速度与运动加速度和合力，不可直接使用, 磁力计测得载体坐标系下的地磁场强度,36%Conveniently iterate top-line alignments,特点1 MEMS微电子机械系统，16位AD采样。,无人机中惯性导航传感器的组成,特点2数据高斯噪声大。,特点3都是载体上的数据。,陀

3、螺仪,陀螺仪（如图所示）是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置。利用其他原理制成的角运动检测装置起同样功能的也称陀螺仪。 陀螺仪的原理就是，一个旋转物体的旋转轴所指的方向在不受外力影响时，是不会改变的。人们根据这个道理，用它来保持方向，制造出来的东西就叫做陀螺仪。陀螺仪在工作时要给它一个力，使它快速旋转起来，一般能达到每分钟几十万转，可以工作很长时间。然后用多种方法读取轴所指示的方向，并自动将数据信号传给控制系统。在现实生活中，陀螺仪发生的进给运动是在重力力矩的作用下发生的。,陀螺仪校准,消除角速度漂移的方法：静止放置后，取100次数据进行平滑滤

4、波处理作为3个轴的Offset。根据陀螺仪对应数据手册中的偏置、噪声和比例因子误差建立陀螺仪的简单特性模型，进行误差公式计算，采集校准数据完成精确校准。,陀螺仪校准前：,陀螺仪校准后：,陀螺仪计算倾角,陀螺仪可以测量角速度,具有高动态特性,但是它是一个间接测量器件,它测量的是角度的导数：角速度。显然我们要将角速度对时间积分才能得到角度，如下公式所示： 看到积分马上就能发现一个致命的问题积分误差,积分误差的来源主要有两个： 1. 一个是积分时间，很难得到一个准确的时间，积分时间 dt越小，输出角度越准。 2. 一个是器件本身的误差，积分之后会累计。总结： 动态测量性能较好，但是有累计误差。例程见

5、：Gyro_GetAngles文件,Angle = 0 Gyro ,加速度计,加速度计是测量运载体线加速度的仪表。在飞机控制系统中，加速度是重要的动态特征校准元件。在惯性导航系统中，高精度的加速度计是最基本的敏感元件之一。在各类飞行器的飞行试验中，加速度计是研究飞行器运动状态的重要工具。 很多时候大家都会被加速度计的名字给误导了，准确来说它测的不是加速度，它检测的是它受到的惯性力(包括重力)。,加速度计计算倾角,加速度计测量的是物体的加速度，我们知道重力加速度是一个物体受重力作用的情况下所具有的加速度。当物体处于静止状态时，加速度计测量出来的值就等于重力加速度 1g, 约等于9.8 米每平方秒

6、。重力加速度 g 的方向总是竖直向下的，通过获得重力加速度在其X轴，Y轴上的分量，我们可以计算出物体相对于水平面的倾斜角度。,加速度计特点,当系统在三维空间做变速运动时，它的输出为轴向的加速度和重力加速度的分量，这就不能准确用于计算物体的姿态和运动状态。 静态时，加速度计计算的东北天倾角比较准确；而动态时，倾角的误差就很大，跟随性也较差。 只靠加速度传感器来估计自己的姿态是很危险而不可取的，所以在姿态计算中我们一般采用陀螺仪的动态数据为主体，而加速度计起到修正陀螺仪积分误差的作用。例程见：Acc_GetAngles文件,磁力计,磁力计，可用于测试磁场强度和方向，定位设备的方位，磁力计的原理跟指

7、南针原理类似，可以测量出当前设备与东南西北四个方向上的夹角。 地球的磁场像一个条形磁体一样由磁南极指向磁北极。在磁极点处磁场和当地的水平面垂直，在赤道磁场和当地的水平面平行，所以在北半球磁场方向倾斜指向地面。用来衡量磁感应强度大小的单位是Tesla或者Gauss（1Tesla=10000Gauss）。随着地理位置的不同，通常地磁场的强度是0.4-0.6 Gauss。 地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。如果保持电子罗盘和当地的水平面平行，那么罗盘中磁力计的三个轴就和这三个分量对应起来。,磁力计校准,() + ()

8、 + () = ,Xoffset=（Xmax+Xmin）/2Yoffset=（Ymax+Ymin）/2Zoffset=（Zmax+Zmin）/2,第一步：水平放置，绕 Z 轴旋转 360 度，采集磁力计 X 轴和 Y 轴的最大最小值。第二步：X 轴与水平面垂直放置，绕 X 轴旋转 360 度，采集磁力计 Z 轴的最大值和最小值。第三步：8 字校准法，在空中任意方位做 8 字晃动，原则上尽量多的让设备法线方向指向空间的所有 8 个象限，直到校准值达到稳定不变时，结束磁校准保存磁标定值。,受到外部磁场干扰后的磁心偏移,磁校准的目的是保持当地环境下的磁心准确，来获得准确的磁场数据。,磁力计计算方位角

9、,水平时：X=X原始-XoffsetY=Y原始-YoffsetZ=Z原始-Zoffset方位角=arctanY/X,Xh=X*cos()+Y*sin()*sin()-Z*cos()*sin()Yh=Y*cos()+Z*sin()考虑到角度的4 个象限，航向角的计算公式如下：for(Xh0,Yh0,Yh0)=180-arctan(Yh/Xh)*180/for(Xh=0,Yh0)=90,倾斜时：横滚角（）Roll 以及俯仰角（）Pitch,例程见：Mag_GetYaw文件, 陀螺仪（MPU6500）测得载体坐标系下的角速度数据,加速度计（MPU6500）测得载体坐标系下重力加速度与运动加速度和合力

10、, 磁力计（LSM303D）测得载体坐标系下的地磁场强度,惯性导航传感器的量程选择与低通滤波器设置, 陀螺仪（MPU6500）原始数据还可以不需要硬件低通，追求数据的实时性,加速度计（MPU6500） 原始数据噪声十分可怕，不低通无法使用, 磁力计（LSM303D）选择性进行低通,惯性导航传感器的量程选择与低通滤波器设置,机架的刚性和材质,电机KV值的大小,桨叶材质与电机的匹配度,惯性导航传感器的噪声源, PHOTOSConveniently iterate top-line alignments for wireless metrics., CUSTOMIZEConveniently ite

11、rate top-line alignments for wireless metrics.,巴特沃斯滤波器的特点是通频带的频率响应曲线最平滑!,36%Conveniently iterate top-line alignments,加速度数据滤波,上图是巴特沃斯滤波器（左上）和同阶第一类切比雪夫滤波器（右上）、第二类切比雪夫滤波器（左下）、椭圆函数滤波器（右下）的频率响应图,Set_Cutoff_Frequency(500, 30, 500 为加速度数据的采样频率为巴特沃斯滤波器的截止频率，一旦改变截止频率，控制参数全部改变, CUSTOMIZEConveniently iterate to

12、p-line alignments for wireless metrics.,加速度数据的二阶巴特沃斯滤波,蓝色为原始数据，绿色为滤波后的数据,位置传感器,无人机除了需要掌握自身的姿态以外，还需要较为准确的绝对位置或相对位置信息去完成悬停、寻迹追踪、巡航等任务。常见位置传感器：超声波传感器气压计GPS全球定位RTK高精度定位射频传感器组摄像头模组面阵雷达红外等感光传感器等,超声波传感器,工作频率： 40KHz感应角度一般为： =30短距离测量范围一般为： 2CM450CM测量精度： 0.3CM 1%声音在空气中的传播速度大约是340m/s，是指声音在15空气中的传播速度。声速随温度的升高而增

13、大，温度每增高1，声音在空气中每秒传播的距离约增加0.6m。关系式 ： v=340(1+0.6t)m/s特点： 适合室内平整环境使用，且需要角度补偿和减震措施。,超声波模块测距,即距离值为：(高电平时间*340m/s)/2,超声波数据的滤波,改进的中位值平均滤波第一种：连续出现过小的数据2.31 2.32 2.26 1.25 1.22 1.31 2.31 2.28 其中1.20 1.21 1.21三组为出错数据第二种：连续出现过大的数据2.32 3.40 3.45 3.48 2.31 2.24 2.33 2.32 其中3.40 3.45 3.48三组为出错数据,1.对采样的样本进行排序2.记录

14、每个数据的原先和排序后的序号3.对排序后的数据两两做差4.寻找出误差突变最大（超过变化阈值）的2个数据原始序号5.取突变前和突变后序号的数据进行平均值滤波,超声波定位,（1）利用3面垂直的墙壁进行定位，要求墙面平整，中间不能有障碍。,（2）在空间某些位置放置发射端，利用散射角，接收主体计算出到每个发射端的距离后，进行坐标转换。点越多越准确，但是计算复杂。,超声波定位示例, + + = () + + = + () + = ,= + = + = + () ,设主体坐标(x,y,z)到三个发射点的距离分别为L1、L2、L3。,超声波室内定位难点,1. 需要准确的航向信息和航向控制。2. 需要实时地对

15、场地坐标系、无人机载体坐标系和东北天坐 标系进行转换。3. 在航向发生偏转时，对控制量（Pitch和Roll）关于场地坐标系（X轴和Y轴）进行分解。,气压计,BMP180/085 MS5611气压范围： 300 . 1100hPa 10 . 1200mbar (+9000m . -500m)误 差： 0.17m 0.1m转换时间： 7.5ms （max） 9.04ms （max）,1Kpa=10百帕=7.5毫米汞柱=7.5mmhg1Bar=0.1MPa=1000mba=1000hpa=75mmhg=1个大气压,硬件上，气压计数据容易受温度和气流影响，有安装要求（放置泡沫棉于传感器上方）。软件上

16、，可以用卡尔曼滤波对高度信息进行处理。,气压计数据处理方法,位置估计算法（通过加速度来估计补偿高度）卡尔曼滤波详细对比试验见Git上的气压计数据融合例程例程见：IIC_MS5611,全球定位,1、美国的GPS系统（全球）2、俄罗斯GLONASS系统3、中国北斗（亚洲）4、欧洲伽利略,Ublox-GPS模块,NEO-M8 模块数据,1、GPS数据有效位 V (数据无效) or A ( 数据有效) 2、经纬度 转换为坐标轴的相对距离 Xnow= (lat2 - lat1)*110946.0Ynow= (lon2 - lon1)* cos(lat2 + lat1)/2)* 0.0174532925)

17、*111318.03、地速 1节 = 1海里/小时 = 0.514米/秒4、航向5、卫星数目 012颗,GPS数据分析与定位,无人机户外位置控制（速度和位置）地速（不准）经纬度转坐标信息（会飘）,加速度与位置信息融合的速度加速度与位置信息融合的位置,卡尔曼滤波或位置估计算法,关键点： 1. 要有准确的航向信息和航向控制。 2. 载体坐标系与东北天坐标系的转换。 3. 无头模式下，需要实时通讯遥控器和无人机之间的航向信息。 4. GPS精度不高，室外定位会有轻微的漂移。例程见：DMA_GPS,高精度RTK厘米级定位,支持信号： BDS、GPS、GNSS等输出格式： NMEA-0183定位精度：

18、平面1cm、高程2cm更新速率： 1Hz、5Hz（高速1020Hz）作业距离： 3公里通讯方式： 蓝牙4.0、WiFi、3G,优势：定位精度高且稳定性强，适用于农业高精度植保、巡检等场合。,RTK高精度定位,First. 路径规划,Second. 无人机编队,UWB （脉冲无线电）原理,UWB 的测距原理是什么？ 双向飞行时间法（TW-TOF, two way-time of flight）每个模块从启动开始即会生成一条独立的时间戳。模块 A 的发射机在其时间戳上的 Ta1 发射请求性质的脉冲信号，模块 B 在 Tb2 时刻发射一个响应性质的信号，被模块 A 在自己的时间戳 Ta2 时刻接收。

19、 以此可以计算出脉冲信号在两个模块之间的飞行时间，从而确定飞行距离 S。S=C* (Ta2-Ta1)-(Tb2-Tb1)(C 为光速),障碍物干扰！,UWB定位,UWB 定位的原理是什么？1) 距离 = 光速 * 时间差 / 2；XY 平面，3 个圆，能够确定一个点；2) XYZ 空间，4 个圆，能够确定一个空间点；3) 现有的算法在Z轴上有误差，所以可以用超声波或者气压计替代Z轴数据。,UWB定位注意事项,UWB解算算法，估算坐标信息。需要校准4个基站之间的实际距离。（天线放置位置不同，测得距离不同）需要对UWB场地坐标、地理坐标系和载体坐标系做转换。,图像采集传感器,功能：1. 坐标信息获

20、取（必须结合高度和倾角）2. 物体和人脸识别3. 颜色识别等4. 视觉里程计特点：易受环境影响微弱干扰（调节色域的阈值）高光,02,姿态解析算法详解,姿态解算中的主角,姿态解算中的传感器的特性：加速度：使用三角公式得到绝对角度。 “动态不稳定，静态稳定”角速度：通过角速度积分累加得到角度。 “动态稳定，长时间不稳定”磁力计：用于计算方位角即航向。 “易受磁干扰，需要倾角补偿” 在现有较成熟的姿态解算算法中，主角还是角速度和加速度，磁力计只是起矫正纠正作用。,姿态融合,在大部分飞控传感器中通常使用的是成本低廉mems传感器，这种传感器的陀螺仪和加速度计的噪声相对来说很大，以常见的陀螺仪为例进行积

21、分1分钟会漂移2度左右。在这种前提下，如果没有磁场和重力来修正三轴陀螺仪的话，那么基本上3分钟以后物体的实际姿态和测量输出姿态就完全变样了。所以我们要了解常用的一些数据融合方法，来获得无人机的姿态。,惯性传感器姿态融合,1. 互补滤波 最简单滤波 2. Mahony滤波 有反馈的互补滤波 3. 扩展卡尔曼滤波 复杂滤波、计算量庞大,无人机中的坐标系,“地理”坐标系地球上的“东北天（ENU）”坐标系“载体”坐标系四旋翼自身的坐标系“场地”坐标系 飞行场地的坐标系,一阶互补滤波,我们已经知道，MPU6500可以获得加速度和角速度数据，但是由于传感器的噪声信号比较大，无法直接通过两个数据来获得该轴上

22、的角度。因此，需要进行互补滤波来近似得到一个比较准确的角度。 互补，即高频信号与低频信号都与我们将求得的量相关，我们将其进行滤波运算，给两部分一个权重得到一个相对准确的值，或者得到一个很好的近似的过程。公式： Angle = a*(Angle + Gyro*dt) + (1-a)*AccAngle,简单互补滤波,要求 : 平放开始计算（尽量与地理坐标系重合）。优点： 算法简单，适用于要求实时精度较低的场合，可使用二阶互补滤波进行优化。缺点： 并没有把传感器的噪声和系统的噪声考虑在内。且互补滤波系数不能随着实际的观测数据做调整（没有反馈和修正）。例程见：Complementary filteri

23、ng_AHRS,无人机姿态的表示方法,在姿态解算过程中，到底用什么表示无人机的姿态呢？姿态表示的方法有很多种，比如欧拉角、四元数、DCM，各有的各的优势。,欧拉角和四元数,欧拉角是用来唯一地确定定点转动明体位置的三个一组独立角参量，由章动角、进动角和自转角组成。在航姿中我们把它们称作俯仰角、翻滚角和偏航角。机体坐标系与地理坐标系之间的夹角就是飞机的姿态角，又称欧拉角。 四元数在航姿中的意义，一个向量（x,y,z）绕角度旋转。 特点：模等于1,DCM方向余弦矩阵,C11 C12 C13 C21 C31 C22 C32 C23 C33 第i行、j列的元素表示地理坐标系i轴和载体坐标系j轴夹角的余弦

24、。所以方向余弦矩阵也能用来表示物体的姿态。,DCM方向余弦矩阵的表示方式,C11 C12 C13 C21 C31 C22 C32 C23 C33 =,q02+q12q22q32 2(1203) 2(13+02) 2(12+03) 2(1302) q02q12+q22q32 2(23+01) 2(23+01) q02q12q22+q32 T,cos cos+ sin+cos cos coscos+ sincos+cossin cos =,方向余弦矩阵的作用,C11 C12 C13 C21 C31 C22 C32 C23 C33 * a b c C11 C12 C13 C21 C31 C22 C3

25、2 C23 C33 T* a b c 载体坐标系转换到地理坐标系 地理坐标系转换到载体坐标系 注：向量(a,b,c)可以为3轴加速度数据、3轴磁力计等。 载体坐标系和地理坐标系之间的转换应用：姿态解算，惯性导航，位置估计等姿态解算的核心在于使用陀螺仪、加速度、磁力计修正方向余弦矩阵,Mahony互补滤波,核心思想： 对于一个确定的向量，用不同的坐标系表示时，它们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，为我们通过这个偏差来修正这个旋转矩阵，矩阵中的元素就可以表示姿态，姿态也就被修

26、改了。核心步骤：加速度数据归一化。将加速度向量(0,0,1)从地理坐标系转换到载体坐标系得到预测向量。将预测加速度向量与实际加速度向量做叉积得到误差（偏差角度的余弦）。将误差做PI调节，补偿叠加到原始角速度数据向量上。通过四元数微分方程，将补偿过的角速度向量转换为四元数向量。将四元数转换为欧拉角。新的四元数重新代入方向余弦矩阵，生产新的方向余弦矩阵，用于数据从地理坐标系到载体坐标系的转换。直到误差为0，方向余弦矩阵正确，姿态数据正确。,DCM方向余弦矩阵表示方法选择,旋转矩阵即方向余弦矩阵的表示方式有3种，但要用于姿态解算，我们需要对比这个方法的优缺点。,欧拉角法存在奇点（万向节死锁），不能全

27、姿态解算；方向余弦可用于全姿态解算但是计算量大；四元数法，计算量较小，无奇点可满足无人机运动过程中的姿态实时解算。,Mahony滤波详解,注：加速度3轴原始数据(ax,ay,az) , v向量为估算的载体加速度向量。加速度数据归一化，转换为单位向量recipNorm = invSqrt(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm;在地理坐标系下，物体在任意位置和姿态下只有一个竖直向下的Z轴重力加速度1g，另外两个轴的数据为0，即向量(0,0,1)，将向量(0,0,1)转换到载体坐标上的

28、向量，方向余弦不同，转换出的向量不同。 * ax ay az vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;,Mahony滤波详解,计算估算的载体加速度向量v 与实际的加速度向量a的叉积（向量积）。 向量积的定义： a b = |a|b|*sin 在这里表示两向量之间的夹角(共起点的前提下)（0 180） 在坐标中可表示为：ab=(aybz -azby)i+(azbx -axbz)j+(axby - aybx)k 所以将a向量与b向量做叉积的公式如下，得到e向量（误差向量）

29、ex = (ay * vz - az * vy); ey = (az * vx - ax * vz); ez = (ax * vy - ay * vx);解析： 当方向余弦矩阵表示正确姿态时，向量v应该与向量a重合，即e向量（误差）的模为0，但是一开始方向余弦矩阵作为初始矩阵需要不停被修正，这里向量积的意义就是通过计算误差来修正矩阵。,Mahony滤波详解,对误差向量e做PI运算，将输出累加到载体坐标系下的角速度数据上，有反馈的对角速度数据进行实时的纠正补偿。 integralFBx += Ki * ex * (1.0f / twosampleFreq);integralFBy += Ki *

30、 ey * (1.0f / twosampleFreq);integralFBz += Ki * ez * (1.0f / twosampleFreq);gx += integralFBx;gy += integralFBy;gz += integralFBz;gx += Kp * ex;gy += Kp * ey;gz += Kp * ez;注： twosampleFreq为采样率，积分项需要乘以相应的积分时间。Kp参数和Ki参数需要整定，Kp过大数据会有震荡，Ki过大也会有震荡，过小导致消除静态误差的时间很长。,Mahony滤波详解,通过四元数微分方程（角速度与四元数的关系），将补偿修正过

31、的角速度数据（g向量）转换成四元数，并将四元数归一化。 tempq0 = q0 + (-q1*gx - q2*gy - q3*gz)*(1.0f / twosampleFreq); tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*(1.0f / twosampleFreq); tempq2 = q2 + (q0*gy - q1*gz + q3*gx)*(1.0f / twosampleFreq); tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*(1.0f / twosampleFreq); recipNorm = invSqrt(tem

32、pq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);q0 = tempq0 * recipNorm;q1 = tempq1 * recipNorm;q2 = tempq2 * recipNorm;q3 = tempq3 * recipNorm;,Mahony滤波详解,通过修正补偿过后的角速度求得的新四元数，将带入方向余弦矩阵，成为一个修正过的新矩阵，即表示无人机的新姿态。再次得到预测的加速度向量v与原始加速度向量a做叉积，不停的循环纠正方向余弦矩阵。将四元数转换为直观的欧拉角表示无人机的航姿，公式如下：,磁力计修正方向余弦矩阵

33、,左侧的图为加速度修正姿态，将平面1与平面2重合即修正完毕。而同一平面上，无法用加速度计对航向进行修正，静止状态下不同航向的3轴数据相同。而右侧的图则通过正北来修正同一平面上的航向漂移，以此来获得准确的航向,磁力计修正方向余弦矩阵,1. 将磁力计的数据归一化，变成单位向量mrecipNorm = invSqrt(mx * mx + my * my + mz * mz);mx *= recipNorm;my *= recipNorm;mz *= recipNorm;思考： 加速度计在静止时，测量的是重力加速度（0,0,1），有大小和方向。同理，地磁计同样测量的是地球磁场的大小和方向，只不过不再是

34、竖直向下的了。我们把磁场数据记做 (bx,by,bz)，将x轴对准正北，by=0，即 (bx,0,bz)但实际上，当地磁场相对东北天的夹角是无法测量的，那我们应该怎么去做磁力计的补偿呢？,磁力计修正方向余弦矩阵,2. 将载体坐标系的磁力计数据转换到地理坐标系下，公式如下： * mx mz hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); hz= 2*mx

35、*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);3. 在地理坐标系下，XOY平面上的向量大小必定相等= 2 + 2 ，因为在磁力计校准后的状态下，固定环境下的磁场是一个固定值（球面上的点到球心的距离为定值）。X轴指向正北的情况下，by=0；bz=hz，我们得到一个新的向量b（地理坐标系下的固定向量）。,磁力计修正方向余弦矩阵,4. 将b向量从地理坐标系转换到载体坐标系得到 (,) , * x 0 z wx = 2*bx*(0.5f - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);wy = 2

36、*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5f - q1q1 - q2q2); 5.计算估算的载体磁力计向量w 与实际的磁力计向量m的叉积（向量积），并叠加到之前的加速度误差上。ex = (ay * vz - az * vy) + (my * wz - mz * wy);ey = (az * vx - ax * vz) + (mz * wx - mx * wz);ez = (ax * vy - ay * vx) + (mx * wy - my * wx);,磁力计修正方向余弦矩阵,6. 对误

37、差向量e做PI运算，将输出累加到载体坐标系下的角速度数据上，有反馈的对角速度数据进行实时的纠正补偿。 integralFBx += Ki * ex * (1.0f / twosampleFreq);integralFBy += Ki * ey * (1.0f / twosampleFreq);integralFBz += Ki * ez * (1.0f / twosampleFreq);gx += integralFBx;gy += integralFBy;gz += integralFBz;gx += Kp * ex;gy += Kp * ey;gz += Kp * ez;注： twosam

38、pleFreq为采样率，积分项需要乘以相应的积分时间。Kp参数和Ki参数需要整定，Kp过大数据会有震荡，Ki过大也会有震荡，过小导致消除静态误差的时间很长。,磁力计修正方向余弦矩阵,7. 通过四元数微分方程（角速度与四元数的关系），将补偿修正过的角速度数据（g向量）转换成四元数，并将四元数归一化。 tempq0 = q0 + (-q1*gx - q2*gy - q3*gz)*(1.0f / twosampleFreq); tempq1 = q1 + (q0*gx + q2*gz - q3*gy)*(1.0f / twosampleFreq); tempq2 = q2 + (q0*gy - q1

39、*gz + q3*gx)*(1.0f / twosampleFreq); tempq3 = q3 + (q0*gz + q1*gy - q2*gx)*(1.0f / twosampleFreq); recipNorm = invSqrt(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);q0 = tempq0 * recipNorm;q1 = tempq1 * recipNorm;q2 = tempq2 * recipNorm;q3 = tempq3 * recipNorm;,磁力计修正方向余弦矩阵,8. 修正补偿过后的角速度求得的新四元数，将带入方向余弦矩阵，成为一个修正过的新矩阵，即表示无人机的新姿态。再次得到预测的向量与原始向量做叉积，不停的循环纠正方向余弦矩阵。9. 将四元数转换为直观的欧拉角表示无人机的航姿，公式如下：, CUSTOMIZEConveniently iterate top-line alignments for wireless metrics.,Mahony滤波中的PI参数整定,上图为合适的PI参数，下图为P调节过大所产生的超调,六轴和九轴姿态结算的优缺点,9轴：有绝对的航向正北，可实现无头模式。6轴：不受磁场干扰，在强磁环境下正常运行。 9轴不建议使用在竞赛！,