精华内容
下载资源
问答
  • 什么是卡尔曼滤波,简单来说,就是结合不同噪声源的数据来估计出状态的最优值。 卡尔曼滤波的应用:1、当不能直接测量得到想要的数据时,应用卡尔曼滤波来间接测量; 2、在有噪声情况下结合不同的传感器来找到数据的...
  • 无人机飞控算法-姿态估计-欧拉角-旋转矩阵-四元数

    千次阅读 多人点赞 2020-12-06 19:39:43
    无人机飞控算法-姿态估计(EKF卡尔曼滤波) 此系列记录了我理解的卡尔曼滤波从0到1的过程,从姿态估计到位置估计,我们从核心点一个个出发,并结合实际模块的应用来一一揭开卡尔曼滤波的神秘面纱。 提示:在系列文章...

    无人机飞控算法-姿态估计

    此系列记录了我理解的卡尔曼滤波从0到1的过程,从姿态估计到位置估计,我们从核心点一个个出发,并结合实际模块的应用来一一揭开卡尔曼滤波的神秘面纱。


    提示:在系列文章中,我参考了大量CSDN,知乎,简书等其他朋友的各种文档资料,并从中获益良多,对此表示非常感谢!在更新的过程中不免出现很多错误,希望大家能够及时指出和交流,一起学习,一起进步!


    前言

    作为一个非控制类出生的通信工程师,半路出家,还是挺困难的,需要学习的知识很多,主要是卡尔曼滤波方面的,包含微积分、线性代数、概率统计论、信号与系统、控制论等等。在无人机里飞控是其中重要的组成部分之一,而飞控的核心就是算法,主要包括姿态估计算法,导航控制算法,PID控制算法,路径规划算法等。此系列从姿态估计算法入手,通过IMU的姿态解算来学习姿态估计算法究竟是为何物。


    推荐:学习基础知识我推荐一下3blue1brown,在B站上可以搜索到他们的课程,讲的很生动,可以帮您快速理解一些核心知识。

    一、姿态估计是什么?

    想象一下,你拿着一根平衡杆,走在独木桥上,你该如何操控你手里的平衡杆才能使你不掉下去呢?从主观意识着想,其实挺简单的,如果身子往左倾斜,我们就把杆子向右移,如果身子往右倾斜,我们就把杆子向左移。要实现这个目的,首先我们是不是要知道身体现在究竟是左倾斜还是右倾斜呢?这就是姿态估计(Attitude Estimator)。那如何检测姿态呢?这就需要传感器了,在人类的身上,大脑充当了这个角色,在机器上,我们通过惯性测量单元(IMU)来测量姿态。

    二、惯性测量单元(IMU)

    1.陀螺仪

    陀螺仪是利用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置。利用其他原理制成的角运动检测装置起同样功能的也称陀螺仪。陀螺仪可感测一轴或多轴的旋转角速度,可精准感测自由空间中的复杂移动动作,因此,陀螺仪成为追踪物体移动方位与旋转动作的必要运动传感器。不像加速器与电子罗盘,陀螺仪不须借助任何如重力或磁场等的外在力量,能够自主性的发挥其功能。所以,从理论上讲只用陀螺仪是可以完成姿态导航的任务的。陀螺仪的特性就是高频特性好,可以测量高速的旋转运动。缺点是存在零点漂移,容易受温度/加速度等的影响。

    2.加速度计

    加速器可用来感测线性加速度与倾斜角度,单一或多轴加速器可感应结合线性与重力加速度的幅度与方向。含加速器的产品,可提供有限的运动感测功能。加速度计的低频特性好,可以测量低速的静态加速度。在我们的飞行器上,就是对重力加速度g(也就是前面说的静态加速度)的测量和分析,其它瞬间加速度可以忽略。记住这一点对姿态解算融合理解非常重要。当我们把加速度计拿在手上随意转动时,我们看的是重力加速度在三个轴上的分量值。加速度计在自由落体时,其输出为0。为什么会这样呢?这里涉及到加速度计的设计原理:加速度计测量加速度是通过比力来测量,而不是通过加速度。

    3.磁力计

    电子罗盘也叫数字指南针,磁力计,是利用地磁场来定北极的一种方法。现在一般有用磁阻传感器和磁通门加工而成的电子罗盘。电子罗盘可由地球的磁场来感测方向。运用电子罗盘的消费性电子产品应用,包含在手机的地图应用程序显示正确方向,或为导航应用程序提供前进方向数据。然而,电子设备或建筑材料的磁场干扰,比地球磁场来得强,导致电子罗盘传感器的输出值,较容易受到各种环境因素的影响,尤其在室内更是如此,因此,电子罗盘须要透过频繁的校正,才能维持前进方向数据的准确度。


    在捷联式惯导中,一般集成6轴陀螺仪和加速度计,有一些也集成了磁力计,但是一般情况下,我们都使用外部磁力计,因为磁力计在内部很容易受到电磁干扰。

    三、欧拉角

    1.坐标系

    在描述姿态角度的时候一定要确定好坐标系旋转轴旋转顺序旋转方向,如果不确定好这些,那最后得到的旋转矩阵一定是千姿百态,保证你看的眼花缭乱,不明所以,一会又对了,一会儿又错了。关于坐标系的概念和分类大家可以去看相关文档,这里我们只讲和飞控相关的坐标系.

    参考坐标系(导航系)

    在飞控里常用的导航坐标系有两种:东北天和北东地。

    东北天

    东北天用ENU表示XYZ轴
    在这里插入图片描述

    北东地

    北东地用NED表示XYZ轴
    在这里插入图片描述

    载体坐标系(机体系)

    在飞控里常用的载体坐标系也有两种:右前上和前右下。

    右前上

    右前上表示载体坐标系的XYZ轴

    前右下

    前右下表示载体坐标系的XYZ轴

    对应关系

    ENU对应右前上
    在这里插入图片描述

    NED对应前右下

    在这里插入图片描述
    一般在惯导里用ENU较多,在控制领域里用的NED较多,飞控里也用的NED坐标系。

    2.欧拉角和姿态角

    欧拉角是描述旋转的一种方式,那姿态角呢?姿态角属于欧拉角的一种特殊形式,我们一般定义姿态角为偏航(Yaw),俯仰(Pitch),横滚(Roll)。在不同的载体坐标系里,姿态角对应也不一样。

    ENU-右前上

    右前上坐标系下,Z对应偏航,Y对应横滚,X对应俯仰。所以在常用的姿态旋转顺序(Yaw-Pitch-Roll)中,对应的旋转方式为(Z-X-Y)。

    NED-前右下

    前右下坐标系下,Z对应偏航,Y对应俯仰,X对应横滚。所以在常用的姿态旋转顺序(Yaw-Pitch-Roll)中,对应的旋转方式为(Z-Y-X)。前右下的旋转如下图:
    在这里插入图片描述

    注意这里的姿态角方向都遵循右手定则

    3.旋转规则

    旋转规则分两种:

    • Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y) 经典欧拉角
    • Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z) 卡尔丹角

    每种规则都使用了3个变量描述三次旋转过程中的旋转角度, 差别在于Proper Euler angles只涉及两个转轴.而Tait–Bryan angles涉及三个转轴。
    一般我们习惯使用卡尔丹角来旋转,在姿态的旋转中,一般使用(Yaw-Pitch-Roll)的旋转顺序。

    旋转轴

    旋转轴分绕固定坐标系旋转和绕动坐标系旋转,前者称为外旋,后者称为内旋。

    • Fixed Angles 外旋
    • Euler Angles 内旋

    外旋(Fixed Angles)

    外旋为绕静止坐标系旋转,也就是每次旋转都绕参考的静止坐标系进行三轴旋转。外旋为旋转矩阵按照先后依次左乘。

    内旋(Euler Angles)

    外旋为动坐标系旋转,也就是每次旋转后绕变化后的坐标系进行三轴旋转。内旋为旋转矩阵按照先后依次右乘。

    在外旋和内旋中有一个特性很重要,这里先提一下:X-Y-Z的外旋等价于Z-Y-X内旋

    4.万向节(Gimbal)

    在欧拉角表示姿态的过程中会出现一个问题,这个问题叫万向锁。

    什么是万向锁

    在网上找了很多,都说的迷迷糊糊的,这里做个最通俗易懂的解释。万向锁存在于特定的旋转顺序中,当第二次进行Y轴旋转90度时就会出现万向锁,就会丢失一个方向的自由度,不太好理解吧?拿一个飞控举例,长边为Y轴,短边为X轴,垂直向下为Z轴。
    飞控先平放在桌面上:
    在这里插入图片描述
    我们先进行Z轴旋转45度:
    在这里插入图片描述
    我们再绕Y轴旋转90度:
    在这里插入图片描述
    我们再绕X轴旋转45度:
    在这里插入图片描述
    注意:此刻你要从世界坐标系下观察,最开始飞控机载坐标系和世界坐标系重合,我们绕Z轴旋转了一次,也就是在水平方向上有了旋转,然后我们绕Y轴旋转90度,也就是我们在Y方向上有了旋转,现在还剩最后一个旋转X,如果我们此刻绕X轴旋转,会发生什么?在机体坐标系下,你会认为我们还是转了啊?但是在世界坐标系下,无论你在X轴怎么转,你所反应出来的效果和最开始第一次在Z轴下的旋转不是一个效果吗?飞控始终在桌面上水平旋转,只是翻了个身而已,始终无法立起来。这样的旋转虽然我是旋转了三次,但是实际上只需要旋转2次就可以达到同样的效果了,所以就没有办法表示第三个维度的旋转。反过来,从姿态A到姿态B,可以通过几种不同的方式旋转得到同样的结果,那么我们的旋转的角度就不唯一,这就是奇异点。
    当然,你可以说我可以不这么转啊?我不转90度就好了吗?当然这是不可能的,旋转都是不可预测的,他可能是任意角度。
    如果这里你还是不懂的话,我们在下一章旋转矩阵里再来解释这个问题!

    四、旋转矩阵(DCM)

    旋转矩阵又称为方向余弦矩阵,前面我们通过欧拉角来表示姿态的旋转,其实我们可以用旋转矩阵来表示旋转。本身矩阵就是为了表示旋转而生的,三个轴的旋转也就是将三个旋转矩阵相乘即可。

    前提

    描述旋转矩阵之前一定切记,一定要先定义好坐标系和旋转规则,否则就会把小白带入大坑深深不能自拔,本章的DCM遵循如下定义:

    • 参考系:ENU或END
    • 载体系:右前上或前右下
    • 旋转顺序:(Yaw-Pitch-Roll)Z-X-Y或Z-Y-X
    • 旋转轴:内旋
    • 旋转方向:右手定则

    以后在默认情况下,我们一般都遵循内旋和右手定则,只是坐标系和旋转顺序会有一些变化。

    DCM推导

    网上有很多DCM的推导文档,很多都没有注明坐标系和内旋外旋,导致推出来的看不懂。
    我推荐一个视频大家可以看看:B站老师讲旋转矩阵

    推导思路

    根据我们之前提到的一个外旋内旋特性:X-Y-Z的外旋矩阵等价于Z-Y-X的内旋矩阵。
    如果我们要推导内旋矩阵,那么推导出外旋的逆方向矩阵就可以了,注意这里的逆方向可不是矩阵转置。你不信可以用Matlab实验一下。为什么要先推外旋呢?因为外旋比较好理解,很直观就可以把三轴的旋转矩阵算出来。

    再谈坐标系

    在推导外旋旋转矩阵时,我们再分析一下坐标系,也就是ENU和NED,我们用图来对比一下两者有何区别:
    在这里插入图片描述

    他们虽然定义的方向不一样,但同在右手定则下旋转的话,对应的顺序其实是一样的!
    在ENU下,绕Z轴旋转,这时候应该是如下图所示:
    在这里插入图片描述
    Z轴朝上,右手定则,绕Z轴转,我们只关注XY平面上发生的变化,整体变化是从XY坐标系变到了X’Y’坐标系,我们简称为XY变化。
    在NED下,绕Z轴旋转,这时候应该是如下图所示:
    在这里插入图片描述
    Z轴朝下,右手定则,绕Z轴转,我们只关注XY平面上发生的变化,整体变化是从XY坐标系变到了X’Y’坐标系,同样是XY变化。

    比较两者,其实是一样的都是XY变化,同理,其他两轴也是如此!

    从这里我们得到一个结论:不管是东北天还是北东地,他们所对应的单轴的旋转矩阵都是一样的,不同的无非就是旋转顺序和坐标系的定义区别而已。也就是ENU的X-Y-Z和NED的X-Y-Z是一样的,其他旋转顺序也是如此,既然外旋一样,那不同坐标系得到的最终内旋矩阵也是一样!

    一般在ENU下我们用Z-X-Y,在NED下我们Z-Y-X旋转。
    三轴基本旋转矩阵(外旋)
    注意这里的三轴是外旋的,而且前面说了,这里不区分坐标系。
    在这里插入图片描述
    虽然旋转矩阵一样,但是我们最好还是注明坐标系,因为后面的一些运算就和坐标系有关系了。接下来我们就列举常用的几种旋转矩阵来说明。

    默认情况下的旋转,参考系为n系,载体系为b系,旋转的时候,参考系不动,载体系进行旋转

    ENU_Z-X-Y_Euler_Angles

    • 参考系(导航系):(ENU)东北天
    • 载体系(机体系):(XYZ)右前上
    • 旋转顺序:Z-X-Y
    • 姿态顺序:(Yaw-Pitch-Roll)偏航-俯仰-横滚
    • 旋转轴:内旋

    要计算内旋的Z-X-Y,我们只需要计算外旋的Y-X-Z即可,也就是计算出ENU_Y-X-Z_Fixed_Angles:
    在这里插入图片描述
    但是内旋的右乘就是从左到右乘了,从这里也可以看出左乘的Y-X-Z等于右乘的Z-X-Y了。

    下面的旋转矩阵是从载体系到参考系的旋转矩阵R_b2n,左上角标有F表示是外旋下的DCM:

    注意:内旋是按照旋转顺序右乘,外旋是按照旋转顺序左乘,所以内旋的Z-X-Y等于外旋的Y-X-Z。

    在这里插入图片描述
    根据外旋内旋规则那么得到内旋的旋转矩阵R_b2n:从载体系到参考系(载体系在参考系中的表示),:
    在这里插入图片描述
    求转置可得到从参考系到载体系的旋转矩阵:
    在这里插入图片描述

    同理我们再给出其他旋转方式的DCM,过程我就不写了,直接给出结果!

    ENU_Z-Y-X_Euler_Angles

    • 参考系(导航系):(ENU)东北天
    • 载体系(机体系):(XYZ)右前上
    • 旋转顺序:Z-Y-X
    • 姿态顺序:(Yaw-Pitch-Roll)偏航-俯仰-横滚
    • 旋转轴:内旋
      此刻我们变成了Z-Y-X旋转,注意此刻我们的姿态旋转顺序变成了(Yaw-Pitch-Roll,Z-Y-X):
      注意这里的Y轴变成了Pitch,这似乎不合之前坐标系的对应关系。这无大碍,看你最后选的坐标系定了,你也可以把Y轴定义为Roll,但是在东北天下,显得怪怪的,萝卜白菜,各有所爱嘛。
      在这里插入图片描述
      这个旋转矩阵是从载体系到参考系:
      在这里插入图片描述

    求转置可得到从参考系到载体系的旋转矩阵:
    在这里插入图片描述

    NED_Z-Y-X_Euler_Angles

    • 参考系(导航系):(NED)北东地
    • 载体系(机体系):(XYZ)前右下
    • 旋转顺序:Z-Y-X
    • 姿态顺序:(Yaw-Pitch-Roll)偏航-俯仰-横滚
    • 旋转轴:内旋
      前面我们说了,旋转矩阵和坐标系无关,所以NED下的ZYX和ENU下的ZYX是一样的哦,唯一不同的就是,姿态的命名和正负方向不一样了,绕X轴变成了横滚,绕Y轴变成了俯仰,Z轴还是偏航。
      在这里插入图片描述
      这个旋转矩阵是从载体系到参考系:
      在这里插入图片描述
      求转置可得到从参考系到载体系的旋转矩阵:
      在这里插入图片描述

    我们主要列出了3个DCM包括ENU下的2种和NED下的1种,这三种是比较常用的,其余的大家有兴趣可以自行推导!

    再谈万向锁

    如果在上一章中,你没有完全弄懂为什么欧拉角会有万向锁的问题,这里我们通过DCM矩阵来分析一下,我们用NED下的Z-Y-X旋转DCM矩阵来分析:
    在这里插入图片描述
    如果我们在Pitch方向上旋转90度,cosPitch=0,sinPitch=1,化简矩阵得到:
    在这里插入图片描述

    通过这个矩阵我们可以看出,不管Y,Z轴如何旋转X轴都不变。

    DCM的实际应用

    讲了那么多,是应该用实践上战场了,看看这个DCM究竟有何用!

    加速度计计算初始姿态角

    一般我们在初始化姿态角的时候,都会用加速度计来初始化飞机的横滚和俯仰角,由于加速度计在Z轴上的转动不会产生变化,所以不能计算偏航角,偏航角需要由磁力计获取!
    同样先确定好坐标系,和旋转顺序。我们为什么选用ENU呢?这和传感器有关,我们截取数据手册加速度的方向定义:
    在这里插入图片描述
    这个IMU的加速度计的坐标系正好符合东北天,所以我们用东北天,如果你系统中使用的是NED,那么这里就要把传感器的Y,Z轴取反,所以第一步确立你的坐标系,以后就统一使用它。

    ENU-Z-X-Y

    在这里插入图片描述

    NED-Z-Y-X

    注意:由于IMU的坐标系和NED不一致,所以我们要把传感器的Y轴和Z轴的值取反,乘以静止加速度向量的时候,Z轴也要取反,所以是乘以[0,0,-1],这一点很重要!不然,你计算出来的姿态角度是不对的!

    在这里插入图片描述

    磁力计计算初始偏航角

    由于加速度计无法感知Z轴变化,所以无法通过加速度计来计算偏航角,还好我们有磁力计,我们可以通过磁力计来计算初始偏航角。参考链接:基于加速度计与磁力计的姿态解算方法(加计补偿偏航)

    磁力计原理

    地磁场是一个矢量,对于一个固定的地点来说,这个矢量可以被分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。如果保持电子罗盘和当地的水平面平行,那么罗盘中磁力计的三个轴就和这三个分量对应起来,如图所示。
    在这里插入图片描述
    在这里插入图片描述
    实际上对水平方向的两个分量来说,他们的矢量和总是指向磁北的。罗盘中的航向角(Azimuth)就是当前方向和磁北的夹角。由于罗盘保持水平,只需要用磁力计水平方向两轴(通常为X轴和Y轴)的检测数据就可以用如下公式计算出航向角。当罗盘水平旋转的时候,航向角在0°~ 360°之间变化。
    在这里插入图片描述

    实际航向

    实际情况下但是我们不能保证当前载体绝对水平,所以我们需要把传感器的数据转换到水平面。这里就要用到当前飞机的姿态角了,我们通过DCM矩阵来完成从载体系到参考系的转换,我们需要的是参考系下的水平分量。
    通过矩阵来表达也就是:
    在这里插入图片描述

    令磁力计测量的数据为:
    在这里插入图片描述
    DCM矩阵为(我们使用的是北东地下的ZYX旋转):
    在这里插入图片描述
    由于Yaw不参与转换,所以设Yaw=0,得到:
    在这里插入图片描述
    整理公式得到:
    在这里插入图片描述

    通过这个旋转,我们就把载体坐标系的数据,变成了水平载体坐标系,也就是载体水平的情况下,磁力计测量的数据值。
    在这里插入图片描述
    主意z轴的正负(原始数据是z轴朝上为正)
    最后得到实际的航向角为:
    在这里插入图片描述

    真实航向

    磁力计所解算的航向角是载体坐标x轴相对于磁北而言的,而真北与磁北之间存在一个磁偏角:
    在这里插入图片描述
    所以载体坐标纵轴相对于真北的航向角为:
    在这里插入图片描述

    计算出来的夹角是正还是负?
    和载体坐标系有关,如果z轴朝下,根据右手定则,那么载体的旋转正方向为顺时针,那么得到的偏航角就是负的。
    在这里插入图片描述

    小结

    在实际的系统中使用的时候一定要确定一种旋转方式,不能混用,后面的章节里我们都使用NED参考系下的ZYX内旋。

    五、四元数

    四元数是一个比较抽象的概念,他是站在四维空间的角度看待三维的旋转变化,比较难以理解。你可以简单的理解为,三维空间中的三轴旋转等价于绕某一个轴进行一个角度的旋转,这是比较好理解的,其实这个和Axis Angle轴角的概念很像了。

    四元数如何表达旋转

    四元数如何表示姿态?三维空间的任意旋转,都可以用绕三维空间的某个轴旋转过某个角度来表示,即所谓的Axis-Angle表示方法。物体从一个姿态变化到另一个姿态,可等效为物体绕了某一个轴通过一次旋转某个角度完成,但实际物体可能经过了多次中间过程才变化到了我们想要的姿态,我们不去关心中间过程,我们只需找到一种变化关系,通过这种变换关系,可求出物体从地理坐标系变到物体坐标系坐标或者从物体坐标系变到地理坐标系的坐标。

    四元数表达式

    我们用罗德里格旋转来用四元数表达姿态:
    四元数特性:
    在这里插入图片描述
    我们这里直接给出四元数下的ZYX旋转矩阵:
    在这里插入图片描述

    注意:这里四元数旋转矩阵是唯一的。但是为什么注明ZYX旋转呢?那是应为,欧拉角和四元数之间的转换是区分旋转顺序的,不同的旋转顺序会生成不同的四元数,你可以通过四元数在线来观察其中的变化,你可以看出同一种旋转结果,只对应一个四元数,确对应了多种的欧拉旋转顺序。

    对四元数矩阵推导感兴趣的朋友参考我这篇文章:

    四元数转欧拉角

    回顾一下,我们前面得到了欧拉角转DCM,现在我们有了四元数转DCM,把两者的矩阵一对应,就可以求解四元数转欧拉角的方程了。

    NED_ZYX_Quat2Angle

    注意:我们用的NED下的ZYX旋转
    在这里插入图片描述
    根据矩阵对应得到:
    在这里插入图片描述

    ENU_ZXY_Quat2Angle

    在这里插入图片描述

    欧拉角转四元数

    Z-Y-X的欧拉角转四元数

    我们用的ZYX旋转四元数

    在这里插入图片描述

    注意:这里的乘法不是普通的乘法规则,需遵守四元数乘法规则,不同旋转顺序得到的结果也不一样,这一点和矩阵乘法相似,最后得到四元数方程为:

    在这里插入图片描述

    Z-X-Y的欧拉角转四元数

    ZXY旋转
    在这里插入图片描述

    小结

    其实这些方程,我们在Matlab中的Aero Toolbox中,可以找到所有的旋转关系对应。

    展开全文
  • 无人机系列之飞控算法

    万次阅读 多人点赞 2017-02-09 16:00:35
    飞控系统的关键算法 1.关键算法流程框图 图9 关键算法流程框图 2.姿态解算 (1)init函数初始化,建立3x3矩阵R。 (2)磁力计修正,得到误差corr:先计算得到误差角度mag_er,再用_wrap_pi函数...

    一.无人机的分类

    按飞行平台构型分类:无人机可分为固定翼无人机,旋翼无人机,无人飞艇,伞翼无人机,扑翼无人机等.


    图1 无人机平台构型
    多轴飞行器multirotor是一种具有三个以上旋翼轴的特殊的直升机。旋翼的总距固定而不像一般直升机那样可变。通过改变不同旋翼相对转速可以改变单轴推进力的大小,从而控制飞行器的运行轨迹.


    图2 多轴飞行器


    图3 各类变模态平台

    二.无人机的系统架构


    图4 无人机系统架构

    三.飞控系统简介

    导航飞控系统之导航子系统功能:向无人机提供位置,速度,飞行姿态,引导无人
    机沿指定航线安全,准时,准确的飞行。
     获得必要的导航要素:高度,速度,姿态,航向
     给出定位信息:经度,纬度,相对位移
     引导飞机沿规定计划飞行
     接收控制站的命令并执行
     配合其它系统完成各种任务

    1.飞控系统功能:

    导航飞控之飞控子系统功能:完成起飞,空中飞行,执行任务,返航等整个飞行过
    程的核习系统,对无人机实现全权控制与管理,是无人机的大脑。
    无人机姿态稳定与控制
     与导航子系统协调完成航迹控制
     起飞与返航控制
    无人机飞行管理
     无人机任务管理与控制
    应急控制

    2.飞控系统--传感器:

     飞控系统常用的传感器包括:
     角速率传感器陀螺仪


    图5 陀螺仪
     加速度传感器


    图5 加速计
     气压计和超声波


    图5 声纳与气压二合一
     GPS


    图6 GPS示意图
     光流

    从二维图像序列中检测物体运动、提取运动参数并且分析物体运动的相关规律
     光流是空间运动物体在观测成像平面上的像素运动的“瞬时速度”
     用于飞行器的动态定位和辅助惯性导航
    Lucas Kanade算法
    这个算法是最常见,最流行的。它计算两帧在时间t到t + δt之间每个像素点位置的移动


    图7 光流算法示意图

     地磁传感器


    图8 磁力计

    四.飞控系统的关键算法

    1.关键算法流程框图


    图9 关键算法流程框图

    2.姿态解算

    (1)init函数初始化,建立3x3矩阵R。
    (2)磁力计修正,得到误差corr:先计算得到误差角度mag_er,再用_wrap_pi函数做约束,再计算corr值,相当于机体坐标系绕地理坐标系N轴(Z轴)转动arctan(mag_earth(1), mag_earth(0))度。
    (3)加速度计修正更新误差corr:将陀螺仪计算得到的矩阵第三行(即重力加速度部分)转换到b系,再将加速度测得重力加速度(_accel - 机体加速度)的数据归一化(本身属于b系),将这两个的值进行叉乘即测得误差。具体过程:归一化的n系重力加速度通过旋转矩阵R左乘旋转到b系,即k为归一化的旋转矩阵R(b-e)的第三行,总的受到合力的方向(_accel)减去机体加速度方向(_pos_acc)得到g的方向,即总加速度(加速度获取)减去机体运动加速度(第五部分)获取重力加速度,然后姿态矩阵的不是行就是列来与纯重力加速度来做叉积,算出误差。因为运动加速度是有害的干扰,必须减掉。算法的理论基础是[0,0,1]与姿态矩阵相乘。该差值获取的重力加速度的方向是导航坐标系下的z轴,加上运动加速度之后,总加速度的方向就不是与导航坐标系的天或地平行了,所以要消除这个误差,即“_accel-_pos_acc”。然后叉乘z轴向量得到误差,进行校准 。
    (4)对误差corr进行PI控制器中的I(积分),得到_gyro_bias,再对_gyro_bias做约束处理。
    (5)使用修正的数据更新四元数,并把_rates和_gyro_bias置零便于下次调用时使用。


    图10 姿态解算

    3.姿态控制

    3.1.姿态角度控制

    【1】计算误差值e_R:
    (1)获取目标姿态target,并构建目标姿态旋转矩阵。
    (2)通过控制四元数获取当前状态的旋转矩阵DCM。
    (3)取两个矩阵中的Z轴向量,即YAW-axis。
    (4)计算roll,pitch误差,得到误差值e_R:通过R_z%R_sp_z叉乘当前姿态的z轴和目标姿态的z轴的误差大小(即需要旋转的角度)并旋转到b系(即先对齐Z轴)。
    (5)计算yaw的权重.
    (6)构造e_R_cp反对称矩阵,通过罗德里格公式旋转得到roll,pitch旋转后的矩阵R_rp。
    (7)计算yaw的误差,进一步更新误差值e_R:roll_pitch旋转后的矩阵的x轴和目标姿态的x轴的误差,乘上yaw的权重。
    【2】计算e_R_d:
    (1)目标姿态旋转矩阵获取四元数。
    (2)对四元数的虚部取出赋值给e_R_d,再对其进行归一化处理。
    (3)对四元数的实部取出。
    (4)计算e_R_d:通过虚部与实部的一系列计算得来。
    【3】计算direct_w
    【4】进一步更新误差值e_R:通过e_R *  (1 - direct_w) + e_R_d * direct_w。
    【5】得到_rates_sp角速度变量:对e_R进行p控制,再进行约束

    3.2.姿态角速度控制

    【1】获取当前角速度值rates:通过_ctrl_state数据结构(当前姿态信息)把需要的有效数据赋值给rates。
    【2】获取目前角速度值_rates_sp。
    【3】计算得到角速度差rates_err。
    【4】对角速度差rates_err进行PD控制,还需要加一个前馈。
    【5】发布控制量_att_control。


    图11 姿态控制

    展开全文
  • 正点原子的飞控算法分析.docx
  • 初学PX4之飞控算法

    万次阅读 2018-05-31 12:00:48
    本篇文章首先简述了下px4和apm调用姿态相关应用程序出处,然后对APM的DCM姿态解算算法参考的英文文档进行了翻译与概括,并结合源代码予以分析,在此之前,分析了starlino的DCM,并进行了matlab的实现,因为它更加...

    通知如果你对本站无人机文章不熟悉,建议查看无人机学习概览!!!
    注意:基于参考原因,本文参杂了APM的算法分析。

    本篇文章首先简述了下px4和apm调用姿态相关应用程序出处,然后对APM的DCM姿态解算算法参考的英文文档进行了翻译与概括,并结合源代码予以分析,在此之前,分析了starlino的DCM,并进行了matlab的实现,因为它更加利于理解。后段时间会对px4的四元数姿态解算进行分析。姿态控制部分描述了串级PID在APM里的实现流程,同样后期会完善对px4的分析。最后针对自己平时使用的一些调试技巧进行了总结。


    姿态出处分析

    1. 下面看下重要的一个脚本/etc/init.d/rc.mc_apps,可以知道姿态估计用的是attitude_estimator_q和position_estimator_inav,用户也可以选择local_position_estimator、ekf2,而姿态控制应用为mc_att_control和mc_pos_control。

      #!nsh
      if param compare INAV_ENABLED 1
      then
          attitude_estimator_q start
          position_estimator_inav start
      else
          if param compare LPE_ENABLED 1
          then
              attitude_estimator_q start
              local_position_estimator start
          else
              ekf2 start
          fi
      fi
      
      if mc_att_control start
      then
      else
          # try the multiplatform version
          mc_att_control_m start
      fi
      
      if mc_pos_control start
      then
      else
          # try the multiplatform version
          mc_pos_control_m start
      fi
      ...
      
    2. 而在ardupilot中,姿态解算与控制算法在ArduCopter.cpp的fast_loop任务中以400Hz的频率运行。

      // Main loop - 400hz
      void Copter::fast_loop()
      {
      
          // IMU DCM Algorithm
          // --------------------
          read_AHRS();
          ...
      }
      
      void Copter::read_AHRS(void)
      {
          ...
      
          ahrs.update();
      }
      

    了解了上面的源码出处后,下面将分具体应用进行分析。


    姿态估算

    DCM_tutorial

    imu_guide/imu_guide中文翻译/dcm_tutorial/wiki资料查询/该部分算法源码参考
    将该算法转换为了matlab实现,想了解的可以查看我的github里的DCM工程,能够更好的理解算法,另外,该matlab实现还有一定的bug,希望各位大神的pull request~
    这部分翻译自dcm_tutorial,并结合源码进行分析,可作为下部分DCM理论介绍的基础哦,所以建议先将这部分看完再往下看~

    DCM矩阵

    • 设机体坐标系为Oxyz,与机体坐标系同x,y,z方向的单位向量为i, j, k。地理坐标系为OXYZ,同理地理坐标系的单位向量为 I, J, K。共同原点为O。如图

      IGG = {1,0,0}TT, JGG={0,1,0}TT , KGG = {0,0,1}TT
      iBB = {1,0,0}TT, jBB={0,1,0}TT , kBB = {0,0,1}TT
      下面将i,j,k向量用地理坐标系表示,首先以i作为一个例子。
      iGG = {ixxGG , iyyGG , izzGG}TT
      其中ixxGG表示的是i向量在地理坐标系X轴上的投影,即
      ixxGG = |i| cos(X,i) = cos(I,i)
      在这个式子中,|i|是i单位向量的范式(长度),cos(I,i)是向量I和向量i夹角的余弦,因此可以这样写:
      ixxGG = cos(I,i) = |I||i| cos(I,i) = I.i
      在这个式子中,I.i是向量I和向量i的点积,由于只是计算点积,我们并不关心向量是在哪个坐标系中测量的,只要都是以同一个坐标系表示即可。所以:
      I.i = IBB.iBB = IGG.iGG = cos(IBB.iBB) = cos(IGG.iGG)
      同样的:
      iyyGG = J.i , izzGG=K.i
      所以i向量可以用地理坐标系表示为:
      iGG= { I.i, J.i, K.i}TT
      同样的,j,k向量可以表示为:
      jGG= { I.j, J.j, K.j}TT , kGG= { I.k, J.k, K.k}TT
      将机体坐标i,j,k的地理坐标表示以矩阵的形式表示为:

      这就是方向余弦矩阵,它是由机体坐标系和地理坐标系向量所有两两向量组合的夹角的余弦组成,可以算出共有9××9种。
      下一节的DCM理论里面有另外一种推理DCM的方法,建议交互思考!

    • 而将地理坐标系在机体坐标系中表示与将机体坐标系在地理坐标系中表示是对称的,所以只需简单交换I, J, K 和 i, j, k即可。
      IBB= { I.i, I.j, I.k}TT , JBB= { J.i, J.j, J.k}TT , KBB= { K.i, K.j, K.k}TT
      转化为矩阵形式为:

      很容易可以发现:
      DCMBB = (DCMGG)TT or DCMGG = (DCMBB)TT,换句话说,这两个矩阵是可以相互转换的。
      也可以发现:
      DCMBB. DCMGG = (DCMGG)TT .DCMGG = DCMBB. (DCMBB)TT = I33
      其中I33为3××3的单位矩阵,换句话说,DCM矩阵是正交矩阵。
      证明如下:

      证明这个我们需要知道这些特性:iGTGT. iGG = | iGG|| iGG|cos(0) = 1, iGTGT. jGG = 0,因为i和j是正交的。

    • 方向余弦矩阵也称为旋转矩阵,如果知道机体坐标,则可以算出任意向量的地理坐标(反之同理),下面以机体坐标系向量使用DCM算出地理坐标作为例子进行推理:
      rBB= { rxxBB, ryyBB, rzzBB}TT
      而rGG = { rxxGG , ryyGG , rzzGG }TT
      现在让我们分析第一个坐标rxxGG:
      rxxGG = | rGG| cos(IGG,rGG),由于坐标系旋转,向量的大小,夹角都不会变,故有:
      | rGG| = | rBB| , | IGG| = | IBB| = 1 , cos(IGG,rGG) = cos(IBB,rBB),所以:
      rxxGG = | rGG| cos(IGG,rGG) = | IBB || rBB| cos(IBB,rBB) = IBB. rBB = IBB. { rxxBB, ryyBB, rzzBB}TT
      由上可知,rBB= { rxxBB, ryyBB, rzzBB}TT,替换得:
      rxxGG = IBB. rBB = { I.i, I.j, I.k}TT . { rxxBB, ryyBB, rzzBB}TT = rxxBB I.i + ryyBB I.j + rzzBB I.k
      同样的思路:
      ryyGG = rxBB J.i + ryBB J.j + rzBB J.k
      rzGG = rxBB K.i + ryBB K.j + rzBB K.k
      转化为矩阵形式为:

      得证。
      同样的思路可以证明:

      也可以这样证明:
      DCMBB rGG = DCMBB DCMGG rBB = DCMGTGT DCMGG rBB = I33 rBB = rBB

    角速度

    • 如下图所示,r为任意的旋转向量,t时刻的坐标为r(t)。时间间隔dt后:r = r (t) , r’= r (t+dt) and dr = r’ – r。

      dt时间后向量r绕着与单位向量u同向的轴旋转了dθθ,停到了向量r'的位置。其中u垂直与旋转的机身,因此u正交于r与r',图中显示了u与u',它们与r和r‘的叉乘结果方向相同。故有
      u = (r x r’) / |r x r’| = (r x r’) / (|r|| r’|sin(dθ)) = (r x r’) / (|r|22 sin(dθ))
      由于旋转并不改变向量的长度,因此有| r’| = |r|。
      向量r的线速度可以表示如下:
      v = dr / dt = ( r’ – r) / dt
      当dθ → 0时,向量r和dr的夹角αα可通过r,r'和dr组成的等腰三角形计算:
      α = (π – dθ) / 2 当dθ→ 0时,α → π/2。
      这告诉我们,当dt → 0时,r垂直于dr,因此r ⊥ v (v和dr的方向是一致的)。
      现在定义角速度向量,其中反应了角度的变化率和旋转轴方向。
      w = (dθ/dt ) u

    • 下面分析w和v之间的关系:
      w = (dθ/dt ) u = (dθ/dt ) (r x r’) / (|r|22 sin(dθ))
      当dt → 0时,dθ → 0,因此sin(dθ) ≈ dθ。化简得:
      w = (r x r’) / (|r|22 dt)
      现在由于 r’ = r + dr , dr/dt = v , r x r = 0,利用叉乘的加法分配率可得:
      w = (r x (r + dr)) / (|r|22 dt) = (r x r + r x dr)) / (|r|22 dt) = r x (dr/dt) / |r|22
      最后得出:
      w = r x v / |r|22
      下面反向推理证明v = w x r
      利用向量的三重积公式:(a x b) x c = (a.c)b – (b.c)a,以及v和r是垂直的,所以v.r = 0
      w x r = (r x v / |r|22-) x r = (r x v) x r / |r|22- = ((r.r) v + (v.r)r) / |r|22- = ( |r|22- v + 0) |r|22 = v
      得证。

    陀螺仪及角速度向量

    • 如果我们定期获取陀螺仪的值,时间间隔为dt,那么陀螺仪将会告诉我们在这段时间,地球绕陀螺仪各轴旋转的度数。
      xx = wxxdt,dθyy = wyydt,dθzz = wzzdt
      其中wxx = wxx i = {wxx , 0 , 0 }TT , wyy = wyy j = { 0 , wyy , 0 }TT , wzz = wzz k = { 0 , 0, wzz }TT
      每次旋转都会产生线性的位移
      dr11 = dt v11 = dt (wxx x r) ; dr22 = dt v22 = dt (wyy x r) ; dr33 = dt v33 = dt (wzz x r) .
      矢量相加:
      dr = dr11 + dr22 + dr33 = dt (wxx x r + wyy x r + wzz x r) = dt (wxx + wyy + wzz) x r
      因此线速度可以表示为:
      v = dr/dt = (wxx + wyy + wzz) x r = w x r
      这里的条件是dt很小才能这么推理,也就是说,dt越大误差也就越大。

    基于6DOF或者9DOF的IMU传感器DCM互补滤波算法

    • 科普:6DOF由三轴陀螺仪和三轴加速度计组成,9DOF由三轴磁力计、三轴陀螺仪和三轴加速度计组成。
      定义右手地理坐标系:I指向北方,K指向顶部,J指向西方。

      IMU组成机体坐标系,IMU包括陀螺仪、加速度计等。
      加速度计能感应重力,重力向量指向地心,与顶部向量KBB方向相反,假如加速度计输出为A = {Axx , Ayy , Azz },则KBB = –A。
      磁力计与加速度计相似,除了磁力计可以感应地理的北方以外,假设正确的磁力计输出为M = {Mxx , Myy , Mzz },由于IBB是指向北方,因此IBB = M.
      现在可以计算出JBB = KBB x IBB。
      所以加速度计和磁力计就能单独的给出DCM矩阵:
      DCMGG = DCMBTBT = [IBB, JBB, KBB]TT
      DCM矩阵能转换机体坐标系中的任意向量到地理坐标系中:
      rGG = DCMGG rBB

    • 加速度计和磁力计都需要初始化校正和纠错。
      陀螺仪没有绝对的方向感,比如它不知道北方和顶部在哪里,但加速度计和磁力计知道,因此当我们知道t时刻的方向,矩阵形式表示为DCM(t),那么我们可以用陀螺仪算出更精确的方向DCM(t+1),所以这就是从带有噪音的加速度计或者有磁场干扰的磁力计估算出来的姿态。
      事实是我们可以利用陀螺仪、加速度计和磁力计融合来估算姿态。下面将介绍这种算法:
      地理坐标系表示的DCM矩阵如下:

      该DCM矩阵的各行为IBB, JBB, KBB向量。我们会将重心放在KBB(由加速度计测定),IBB(由磁力计测定)上。 JBB可以由JBB = KBB x IBB计算出来。
      假设机体坐标的顶部向量在t00时刻表示为KBB00,陀螺仪的输出为 w = {wxx , wyy , wzz },一段时间后,该顶部向量表示为KBB1G1G
      KBB1G1G ≈ KBB00 + dt v = KBB00 + dt (wgg x KBB00) = KBB00 + ( dθgg x KBB00)
      其中dθgg = wggdt为三个轴角度的变化向量,这意味这在dt时间内KBB向量在3个轴改变的角度。wgg为陀螺仪测得的角速度。
      程序如下:

          //---------------
          //dcmEst
          //---------------
          //gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation 
          //about a fixed earth's (global) frame, if we look from the perspective of device then
          //the global vectors (I,K,J) rotation direction will be the inverse
          float w[3];                 //gyro rates (angular velocity of a global vector in local coordinates)
          w[0] = -getGyroOutput(1);   //rotation rate about accelerometer's X axis (GY output) in rad/ms
          w[1] = -getGyroOutput(0);   //rotation rate about accelerometer's Y axis (GX output) in rad/ms
          w[2] = -getGyroOutput(2);   //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
          for(i=0;i<3;i++){
              w[i] *= imu_interval_ms;    //scale by elapsed time to get angle in radians
              //compute weighted average with the accelerometer correction vector
              w[i] = (w[i] + ACC_WEIGHT*wA[i] + MAG_WEIGHT*wM[i])/(1.0+ACC_WEIGHT+MAG_WEIGHT);
          }
      

      很显然,还可以通过另外的方式估算KBB。如加速度估算值KBB1A1A,如下推理:
      w-aa = KBB00 x vaa / | KBB00|2- 这个在上面的角速度部分得到了证实。
      其中 vaa = (KBB1A1A- – KBB00) / dt,vaa为KBB00的线速度,且| KBB00|22-- = 1 ,故可以这么计算:
      aa -= dt waa = KBB00 x (KBB1A-1A- – KBB00) = KBB00 x KBB1A-1A- – KBB00 x KBB00 = KBB00 x KBB1A-1A- - 0 = KBB00 x KBB1A-1A-
      程序如下:

          //---------------
          //Acelerometer
          //---------------
          //Accelerometer measures gravity vector G in body coordinate system
          //Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
          //K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)
      
          //Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
          Kacc[0] = -getAcclOutput(0);    
          Kacc[1] = -getAcclOutput(1);
          Kacc[2] = -getAcclOutput(2);
          vector3d_normalize(Kacc);
          //calculate correction vector to bring dcmEst's K vector closer to Acc vector (K vector according to accelerometer)
          float wA[3]; 
          vector3d_cross(dcmEst[2],Kacc,wA);  // wA = Kgyro x  Kacc , rotation needed to bring Kacc to Kgyro
      

      可以通过融合KBB1A1A 和KBB1G1G 计算新的估算值KBB1 ,首先通过求dθa和dθg的加权平均来求dθ:
      dθ = (saa dθaa + sgg dθgg) / (saa + sgg-)
      为什么要求dθ,因为可以同时求得:
      KBB11 ≈ KBB00 + ( dθ x KBB00)
      IBB11 ≈ IBB00 + ( dθ x IBB00)
      JBB11 ≈ JBB00 + ( dθ x JBB00)
      由于IBB, JBB, KBB是相互联系的,所以跟踪相同的dθ。
      到目前为止,我们都没有提及磁力计,一个原因是6DOF的IMU是没有磁力计的,这样也可以使用,只是航向会产生飘移,因此我们可以使用一个虚拟的磁力计,代码中会有体现的。磁力计与加速度计相似:
      mm -= dt wmm = IBB00 x (IBB1M-1M- – IBB00)
      算入加权平均为:
      dθ = (saa dθaa + sgg dθgg + smm dθmm) / (saa + sgg +- smm)
      更新DCM矩阵:
      IBB11 ≈ IBB00 + ( dθ x IBB00) , KBB11 ≈ KBB00 + ( dθ x KBB00) 和 JBB11 ≈ JBB00 + ( dθ x JBB00)
      下面通过计算JBB11 = KBB11 x IBB11,判断估算后的值KBB11是否垂直IBB11。

    • 了确保估算后的值是否还是正交的,如下图,假设向量a,b是几乎垂直的,但不是90°,我们可以找到一个向量b’ 与a垂直,这个b’向量可以通过求 c = a x b,再求 b’ = c x a 得到,可以看出b'是正交于a和c的,因此b'是校正后的向量。

      利用向量的三重积公式展开,且a.a = |a| = 1:
      b’ = c x a = (a x b) x a = –a (a.b) + b(a.a) = b – a (a.b) = b + d,其中d = – a (a.b)是校正量,平行于a,方向取决于a和b的夹角。
      上面的情况是a向量固定,b向量得到校正,下面分析对称的情况,a得到校正,b固定:
      a’ = a – b (b.a) = a – b (a.b) = a + e,其中e = – b (a.b)
      再下面考虑这两个向量都有误差,都得到一半的校正得:
      a’ = a – b (a.b) / 2
      b’ = b – a (a.b) / 2
      所以得到一个相对简单的公式:
      Err = (a.b)/2
      a’ = a – Err * b
      b’ = b – Err * a
      现在我们可以更新DCM矩阵的 IB1, JB1向量。
      Err = ( IBB11 . JBB11 ) / 2
      IBB11’ = IBB11 – Err * JBB11
      JBB11’ = JBB11 – Err * IBB11
      IBB11’’ = Normalize[IBB11’]
      JBB11’’ = Normalize[JBB11’]
      KBB11’’ = IBB11’’ x JBB11’’
      其中Normalize[a] = a / |a|,单位化。

      代码如下:

      //bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
      void dcm_orthonormalize(float dcm[3][3]){
          //err = X . Y ,  X = X - err/2 * Y , Y = Y - err/2 * X  (DCMDraft2 Eqn.19)
          float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
          float delta[2][3];
          vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
          vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
          vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
          vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));
      
          //Z = X x Y  (DCMDraft2 Eqn. 20) , 
          vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
          //re-nomralization
          vector3d_normalize((float*)(dcm[0]));
          vector3d_normalize((float*)(dcm[1]));
          vector3d_normalize((float*)(dcm[2]));
      }
      

    DCM理论

    注意:这部分属于APM源码里px4姿态解算部分。
    资料翻译解读自DMCDraft2.pdf(翻译不妥请谅解,欢迎提意见,另外该理论文档已启动翻译,如果你想参与请点击这里),并结合文档分析了APM的姿态源码部分,目前还有drift_correction函数未进行整理!

    前言

    • 使用矩阵来控制和导航,元素包括陀螺仪,加速度计和gps信息。
    • 总的来说,DCM工作如下:

      • 陀螺仪作为主要的方向信息来源,通过整合一个非线性微分运动方程,表明飞机方向的变化率与旋转速率及它当前的方向之间的关系。
      • 意识到积分过程中的积分误差将渐渐的违反DCM必须满足的正交约束,我们对矩阵的元素进行规则的小调整满足约束。
      • 由于数字误差,陀螺仪漂移,陀螺仪偏移量将逐渐积累在DCM中的元素的误差,我们使用参考向量来检测误差,以及在检测到的误差和和第一步的陀螺仪输入中加一个比例积分负反馈控制器来在建立之前消除误差。gps是用来检测偏航误差,加速度计被用来检测俯仰和滚动。
        整个过程如下:

      代码实现概览:

       // Integrate the DCM matrix using gyro inputs
       matrix_update(delta_t);
      
       // Normalize the DCM matrix
       normalize();
      
       // Perform drift correction
       drift_correction(delta_t);
      

    方向余弦矩阵介绍

    • 所有这一切都与旋转有关。
    • 有几种方法可以做到,比如旋转矩阵和四元数。这两种方法在实现上具有相似的地方,都是尽量准确的表示旋转。四元数只需要4个值,而旋转矩阵需要9个,在这方面四元数具有优势。而旋转矩阵很适合用来导航和控制。
    • 旋转矩阵用来描述一个坐标系相对于另一个坐标系的方向。在一个系统中的向量可以通过乘以旋转矩阵转变到另一个系统中,如果是相反方向旋转则乘以旋转矩阵的逆矩阵,也是它的转置(交换行和列)。单位向量在控制和导航运算中将非常有用,因为它们的长度为1。因此他们能被用于交积和叉积中来获得各种正弦或余弦角。

    • 随着飞机的飞行,我们可以用位置(重心的移动)和朝向(绕着重心方向的变化)了描述它的运动,类似这种变换我们称为刚体变换。通过指定一个轴的旋转来描述其相对于地球的方向。例如将飞机开始放在一个标准方向,然后将其旋转,它将指向另外一个实际的方向,也就是说任何其他的方向都可以通过标准方向的旋转描述。
      旋转组是所有可能的旋转的组。它被称为一组,因为在该组中的任何2个旋转可以组成一个组中的另一个旋转,每一个旋转有一个逆旋转。这里有一个单位旋转。
      旋转组应该得到重视的原因是,你能通过最少的近似来在各个方向控制和导航飞机,包括各种特技。
      基本的想法是,定义了你的飞机的方向的旋转矩阵,可以通过结合描述旋转运动学的非线性微分方程得到。这个结合可以通过一系列的旋转组合完成,也就是两矩阵相乘,这是两个矩阵依次执行的结果。
      然而,数值积分引入的数值误差,并不会产生与符号积分相同的结果。精确的陀螺仪信号的符号积分将产生完全正确的旋转矩阵。数值积分,即使我们有精确的陀螺仪信号,也会引入2种数值误差:

      • 积分误差。数值积分采用有限时间步长和在有限采样速率下采样数据。因为是假定在时间步长内旋转速度是恒定的,这将引入了一个与旋转加速度成比例的误差。
      • 量化误差。不管你用什么代表值,数字表示是有限的,所以有一个量化误差,从模数转换开始,以及所有计算没有保留结果所有位时。
    • 旋转矩阵的一个关键特性是它的正交性,这意味着如果2个向量在一个参照系中是垂直的,它们在每一个参照系中都是垂直的。另外,在每一个参照系中向量的长度是一样的。数值误差可能违反此特性。在许多空间系统中,利用方向余弦矩阵把矢量从一个笛卡尔坐标系变换到另一个笛卡尔坐标系。理想的方向余弦矩阵应当是正交的,而实际上,通过计算得到的矩阵由于种种误差(如计算方法误差、舍入误差等)而失去了正交性,造成变换误差,影响系统精度。于是有必要按某种最优方式,恢复其正交性。矩阵正交化的迭代法有多种,但都计算较繁、运算量大。对于需要把计算得到的方向余弦矩阵周期性地正交化的场合(如捷联式惯导系统),大的运算量将给计算机实时计算带来困难。例如,即使行和列都应该代表单位向量,它们的大小应该等于1,但数值误差可能导致它们变得更小或更大。最终他们可以缩小到零,或去无限。行和列应该是垂直于彼此,数值误差可能导致他们“倾斜”到对方,如下图所示:

    • 旋转矩阵有9个元素。实际上,只有3个是独立的。旋转矩阵的正交特性在数学术语方面意味着矩阵的任何一对行或列都是垂直的。并且在每个列(或行)的元素的平方和等于1。所以这九个元素中有六个约束条件。

    • 反对称矩阵定义是:A=-A’(A的转置前加负号),它的第Ⅰ行和第Ⅰ列各数绝对值相等,符号相反。且主对角线上的元素为均为零。一个小的旋转可以用如下的反对称矩阵来描述:

    • 在我们的例子中,运动学与刚体旋转的含义有关。它的结果是一个非线性微分方程,描述了刚体在其向量旋转速度方面的时间演化。方向余弦矩阵都是关于运动学的。

    • 控制和导航可以在笛卡尔坐标系使用DCM完成叉积和点积运算。下面是具体步骤:

      • 要控制飞机的俯仰,你需要知道这架飞机的俯仰姿态,你可以通过把飞机的翻滚轴与地面垂直做点积。
      • 要控制飞机的翻滚,你需要知道这架飞机的倾斜姿态,你可以通过把飞机的俯仰轴与地面垂直做点积。
      • 要航向,你需要知道你这架飞机相对于你想要去的方向的偏航姿态,可以通过飞机的翻滚轴与想要去的方向的向量做叉积得到。如果是去相反的方向,则是点积运算。
      • 判断飞机是否倒过来,可以通过判断飞机偏航轴与垂直的点积符号,如果小于0,则是朝下的。
      • 计算飞机绕垂直轴的旋转速度,将陀螺仪的旋转矢量转换为地理参考坐标系,然后与垂直轴做点积。
    • 下面将进行深入的理论研究。

    • 确定一个合适的坐标系统描述飞机的运动是必要的。对于大多数处理飞机运动的问题,采用了双坐标系。一个坐标系是固定在地球上的,可以被认为是是一个惯性坐标系,是为了飞机运动分析的目的。另一个坐标系是固定在飞机上的,被称为机体坐标系。图2显示了两右手坐标系:

      其中 xe、ye、ze 是地球坐标系统,ze 指向地心,xe 指向正东方,ye 指向正北方;xb、yb、zb 为机体坐标系。

    • 飞机的方向经常被描述为三个连续的旋转,其顺序是重要的。旋转角被称为欧拉角。假设机体坐标如下:

      进行如下的旋转就可以得到上面图2的结果:

      分析:第一步:假设我站在机体坐标中,我需要通过先绕 Xb 轴旋转 ΦΦ ,再旋转 Yb 轴旋转 θθ,最后绕 Zb 轴旋转 ψψ,回到地球坐标系;先求出每次旋转的矩阵。
      如果绕机体 X 轴旋转的角度为 ΦΦ,那么

      这里是怎么得来的呢?先说一下什么是旋转矩阵?如下图所示,我们假设最开始空间的坐标系也就是机体坐标系XAA,YAA,ZAA就是笛卡尔坐标系,这样我们得到空间A的矩阵VAA={XAA,YAA,ZAA}TT,其实也可以看做是单位阵E。进过旋转后,空间A的三个坐标系变成了图1中红色的三个坐标系XBB,YBB,ZBB,得到空间B的矩阵VBB={XBB,YBB,ZBB}TT。我们将两个空间联系起来可以得到VBB=R·VAA,这里R就是我们所说的旋转矩阵。

      由于XAA={1,0,0}TT,YAA={0,1,0}TT,ZAA={0,0,1}TT,结合下图可以看出,旋转矩阵R就是由XBB,YBB,ZBB 三个向量组成的。讲到这里,大家应该会发现旋转矩阵R满足第一个条件,因为单位向量无论怎么旋转长度肯定不会变而且向量之间的正交性质也不会变。那么旋转矩阵就是正交阵!不过这还不能说明问题,下面我更进一步利用数学公式进行证明。

      进一步讨论之前,我们先说两点数学知识。(1)点乘(dot product)的几何意义:如下图,我们从点乘的公式可以得到α·β相当于β的模乘上α在β上投影的模,所以当|β|=1时,α·β就是指α在β上投影的模。这一点在下面的内容中非常重要,之所以叫余弦矩阵的原因就是这个。(2)旋转矩阵逆的几何意思:这个比较抽象,不过也好理解。旋转矩阵相当于把一个向量(空间)旋转成新的向量(空间),那么逆可以理解为由新的向量(空间)转回原来的向量(空间)。(3)向量是特殊的矩阵,只有一行或一列的矩阵称为向量。向量有叉乘和点乘。矩阵也有,但意义不一样,矩阵还有反对称,逆矩阵等。

      所以上面的公式解析如下:

      同理,其他方向的旋转计算如下:
      如果绕机体 Y 轴旋转的角度为 θθ,那么

      如果绕机体 Z 轴旋转的角度为 ψψ,那么

      第二步:由于站在机体坐标上需要按照 X->Y->Z 轴的顺序,经过 3 次旋转,才能回到地球坐标系;反过来如果站在地球坐标系,则需要经过 Z->Y->X 的三次旋转才能到达机体坐标系。因此我们可以列出从地球坐标系到机体坐标系的DCM矩阵,换句话说此DCM矩阵就是机体坐标在地理坐标系中的表示,其中Φ,θ,ψΦ,θ,ψ为机体在地理坐标系中的姿态角。

      L(Φ,θ,ψ)=L(ψ)?L(θ)?L(Φ);L(Φ,θ,ψ)=L(ψ)?L(θ)?L(Φ);
      矩阵的乘法计算得:

    • 方向余弦矩阵:向量的某些类型,如方向,速度,加速度,和转换,(动作)可以转化为旋转参考系中的一个3x3的矩阵。我们感兴趣的是机体参考系和地面参考系。它可以乘以一个向量的方向余弦矩阵旋转:

    • 由上面的分析可知,方向余弦矩阵与欧拉角之间的关系为:

    • 方程1方程2表明了如何将机体坐标系中测得的向量转换的地理坐标系中。方程1是以方向余弦角的形式,而2为欧拉角。

      以上整个求解过程是对 matrix3.cpp 代码中 from_euler 函数的解析:

      // create a rotation matrix given some euler angles
      // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
      template <typename T>
      void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
      {
          float cp = cosf(pitch);//pitch 表示俯仰相对于地球坐标系的角度值
          float sp = sinf(pitch);
          float sr = sinf(roll);//roll 表示横滚相对于地球坐标系的角度值
          float cr = cosf(roll);
          float sy = sinf(yaw);//yaw 表示偏航相对于地球坐标的角度值
          float cy = cosf(yaw);
      
          a.x = cp * cy;
          a.y = (sr * sp * cy) - (cr * sy);
          a.z = (cr * sp * cy) + (sr * sy);
          b.x = cp * sy;
          b.y = (sr * sp * sy) + (cr * cy);
          b.z = (cr * sp * sy) - (sr * cy);
          c.x = -sp;
          c.y = sr * cp;
          c.z = cr * cp;
      }
      

      其中a,b,c为类定义的私有变量---向量。
      通过不同的旋转顺序可以得到不同的旋转矩阵,如果从地球坐标系到体坐标系,按照 Z->X->Y 轴的顺序旋转可以得到from_euler312函数,这里就没做具体讲解。

      问题:反过来也就可以通过方向余弦矩阵来求出旋转角

      函数 to_euler 式通过上面的 3 个公式求出对应的角度的

      // calculate euler angles from a rotation matrix
      // this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
      template <typename T>
      void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
      {
          if (pitch != NULL) {
              *pitch = -safe_asin(c.x);
          }
          if (roll != NULL) {
              *roll = atan2f(c.y, c.z);
          }
          if (yaw != NULL) {
              *yaw = atan2f(b.x, a.x);
          }
      }
      
    • 地理坐标系中向量的每个分量等于相对应的旋转矩阵的行与机体坐标向量的点积。计算旋转矩阵需要9个乘法和6个加法运算。方程3是方程1的复述,用乘法展开向量和矩阵的元素。所以如果知道机体坐标向量,即可得地理坐标向量的大小:

    • 需要注意的是,矩阵R不一定是对称的。R矩阵的三列对应于机体坐标的三个轴向量到地理坐标的变换。R矩阵的三行则对应于地理坐标三个轴向量到机体坐标的变换。该R矩阵描述了所有机体相对于地球方向的信息。R矩阵也称为方向余弦矩阵,因为每个分量都是机体坐标轴与地理坐标轴夹角的余弦,通过看推理余弦矩阵部分可以看出来。

    • 矩阵的转置,特别在旋转矩阵中,表示为RTRT,通过交换行和列得到。一般来说,一个方形矩阵的逆矩阵如果存在的话,表示为R?1R?1。矩阵的逆乘以矩阵得到的是单位矩阵。(单位矩阵就是对角线上元素为1,其余为0,单位矩阵乘以任何矩阵得到它本身),对于旋转矩阵来说,逆就等于它的转置。

    • 之所以旋转矩阵逆就等于它的转置考虑到了对称性的情况。旋转矩阵的元素都是机体轴与地理坐标轴之间的余弦值,相反的情况就相当于交换地理坐标和机体坐标的角色,也就是说交换行与列,这跟转置是一样的。实际上这又和正交条件达成一致:

      • 正交矩阵每一列都是单位矩阵,并且两两正交。最简单的正交矩阵就是单位阵。
      • 正交矩阵的逆(inverse)等于正交矩阵的转置(transpose)。同时可以推论出正交矩阵的行列式的值肯定为正负1的。
      • 正交矩阵满足很多矩阵性质,比如可以相似于对角矩阵等等。

      如下:

      这个方程用来证明矩阵的逆的矩阵的转置。

    • 旋转矩阵的一个非常有用的特性是,我们可以组成旋转。

      这里特别需要小心运算顺序,因为它的效果是完全不一样的。

    • 另外这里还有一些有用的特性,如:

    • 下面介绍一下点乘和叉乘。

    • 这里有两个DCM计算里边用到的向量运算——点乘和叉乘。点乘是表量运算,A向量作为行向量,B向量作为列向量。

      可以证明向量的点乘等于两个向量的长度乘以它们角度的余弦值。

      所以向量的点乘是对称的。A ?? B = B ?? A

      而两个向量的叉乘是一个向量。它的元素是这样计算的:

      从物理意义上来分析:

      所以叉乘是反对称的,A ×× B = - B ×× A

    方向余弦矩阵的更新

    • 下面到了DCM算法的核心部分——由陀螺仪计算方向余弦矩阵。
    • 核心概念:非线性微分方程——方向余弦的变化速率与陀螺仪之间的关系。我们的目标是计算方向余弦而不是任何违反方程非线性的近似解。目前,我们假设陀螺仪信号没有错误。稍后我们将解决陀螺仪漂移问题。
      不像机械陀螺,我们不能通过简单地积分陀螺仪信号得到角度。一个有名的运动学公式,关于旋转向量的变化率和它的旋转之间的关系:

      下面解释一下这个公式,如下图所示,r为任意的旋转向量,t时刻的坐标为r(t)。时间间隔dt后:r = r (t) , r’= r (t+dt) and dr = r’ – r。
      dt时间后向量r绕着与单位向量u同向的轴旋转了dθθ,停到了向量r'的位置。其中u垂直与旋转的机身,因此u正交于r与r',图中显示了u与u',它们与r和r‘的叉乘结果方向相同。故有
      u = (r x r’) / |r x r’| = (r x r’) / (|r|| r’|sin(dθ)) = (r x r’) / (|r|2 sin(dθ))
      由于旋转并不改变向量的长度,因此有| r’| = |r|。
      向量r的线速度可以表示如下:
      v = dr / dt = ( r’ – r) / dt
      v = w x r
      故可以得出上面的公式。

      有一个需要注意的地方:

      • 微分方程是非线性的。旋转向量输入是与我们要进行积分的变量进行叉乘。因此,任何线性的方法都只是一种近似。
      • 两个向量都应该在同一个坐标系中测量。
      • 因为叉乘是不对称的,所以我们需要保存结果,然后改变它的方向。
    • 如果知道了初始状态和旋转向量的时间,我们可以通过方程11的数值积分来跟踪旋转向量。

      将方程13用于R矩阵的行或列中,可看作成旋转向量。
      这里遇到的第一个问题是,我们要跟踪的向量和旋转向量不是在同一坐标系做测量的。理想情况下,我们都是在地理坐标轴中跟踪机体坐标轴,但是陀螺仪是在机体坐标中测量的。一个简单的方法是通过对称性解决,地理坐标在机体坐标中旋转和机体坐标在地理坐标中旋转是相反的,所以只要改变陀螺仪的符号就好了,更加方便的方法是,交换叉乘的顺序就好了。

      这里的向量代表的是方程1中R矩阵的行。下面的问题是怎么实施方程14,回归到方程14的微分方程形式:

      由这里可知方向余弦矩阵R的行都是通过rearth(t)×dθ(t)rearth(t)×dθ(t)积分得到的。
      所以根据陀螺仪的角度值,来计算当前机体的姿态 DCM矩阵,其使用的方法是:机体坐标的每个轴的向量与 g(陀螺仪改变的角度向量)求叉积,这里求的是角度改变后,姿态在各个方向上的变化量,所以最后使用了矩阵的加法。源码matrix3.cpp中的函数体现如下:

      // apply an additional rotation from a body frame gyro vector
      // to a rotation matrix.
      template <typename T>
      void Matrix3<T>::rotate(const Vector3<T> &g)
      {
          Matrix3<T> temp_matrix;
          temp_matrix.a.x = a.y * g.z - a.z * g.y;
          temp_matrix.a.y = a.z * g.x - a.x * g.z;
          temp_matrix.a.z = a.x * g.y - a.y * g.x;
          temp_matrix.b.x = b.y * g.z - b.z * g.y;
          temp_matrix.b.y = b.z * g.x - b.x * g.z;
          temp_matrix.b.z = b.x * g.y - b.y * g.x;
          temp_matrix.c.x = c.y * g.z - c.z * g.y;
          temp_matrix.c.y = c.z * g.x - c.x * g.z;
          temp_matrix.c.z = c.x * g.y - c.y * g.x;
      
          (*this) += temp_matrix;
      }
      
    • 还有一件事需要做,陀螺仪漂移将在后面进行。我们需要通过比例积分补偿反馈控制器来添加旋转速率校准到陀螺仪测量的数据上,以此产生最优的角速率估计。

      基本上,我们的GPS和加速度计的参考向量被用来计算旋转误差,并通过反馈控制器输入计算,然后更新原有计算。

    • 我们可以把方程15转化为矩阵的形式,这里推导有点复杂,可以了解下矢量阵或者summer的文章,如下:

      方程17就是从陀螺仪信号更新方向余弦矩阵,其对角线上的1就为方程15的第一个条目,其余的为第二个条目,这种方法是用矩阵的乘法实现的,它包含27个乘法和18个加法。正好适合dsPIC30F4011,因为它支持矩阵乘法运算,如果芯片不支持,也可以用方程15的积分形式实现。故在apm里采用的是积分形式累加的。矩阵更新的源码体现如下:

      // update the DCM matrix using only the gyros
      void
      AP_AHRS_DCM::matrix_update(float _G_Dt)
      {
          // note that we do not include the P terms in _omega. This is
          // because the spin_rate is calculated from _omega.length(),
          // and including the P terms would give positive feedback into
          // the _P_gain() calculation, which can lead to a very large P
          // value
          _omega.zero();
      
          // average across first two healthy gyros. This reduces noise on
          // systems with more than one gyro. We don't use the 3rd gyro
          // unless another is unhealthy as 3rd gyro on PH2 has a lot more
          // noise
          uint8_t healthy_count = 0;
          Vector3f delta_angle;
          for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
              if (_ins.get_gyro_health(i) && healthy_count < 2) {
                  Vector3f dangle;
                  if (_ins.get_delta_angle(i, dangle)) {
                      healthy_count++;
                      delta_angle += dangle;
                  }
              }
          }
          if (healthy_count > 1) {
              delta_angle /= healthy_count; //获取角度变化量
          }
          if (_G_Dt > 0) {
              _omega = delta_angle / _G_Dt;
              _omega += _omega_I;
              _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt); //将角度变化量与旋转向量进行叉乘,然后累加
          }
      }
      

    再归一化重整

    • 由于上述的公式是在dt非常小的情况下才误差比较小,这种数字误差将驱使方程4的正交条件逐渐不满足,导致坐标系的两个轴不再能描述刚体。值得幸运的是,数字误差累加的很慢,我们完全可以提前采取办法解决它。
      我们将这种解决办法称为归一化,我们设计了几种方法,模拟实现都可行,最后选择了一种最优的方法。
    • 首先计算矩阵列向量X、Y的点乘,理论上应该等于0,所以它的结果可以看出向量偏了多少。

      将这个误差平分到X、Y向量:

      可以将方程19代人方程18中,验证正交性误差大大减少了。记住R矩阵的行与列的范数为1,将误差平分给两个轴比只分个一个产生较低的残余误差。

    • 下一步就是调整Z列向量正交于X和Y。这个很简单,只要进行叉乘就可以了:

    • 最后一步为,确保R矩阵的各列向量的模为1,一种方法可以通过平方根来求,但是这里有一种更加简单的办法,考虑到这个模不会与1有太大差别,这里可以使用泰勒展开。

      方程21做的事情就是调整各列向量的模为1。展开为3减去向量模的平方,乘以1/2,再乘以这个向量。所以计算更加简单。
      源码实现如下:

      /*************************************************
      -  Direction Cosine Matrix IMU: Theory
      -  William Premerlani and Paul Bizard
       *
      -  Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
      -  to approximations rather than identities. In effect, the axes in the two frames of reference no
      -  longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
      -  simple matter to stay ahead of it.
      -  We call the process of enforcing the orthogonality conditions òrenormalizationó.
       */
      void
      AP_AHRS_DCM::normalize(void)
      {
          float error;
          Vector3f t0, t1, t2;
      
          error = _dcm_matrix.a * _dcm_matrix.b;                                              // eq.18
      
          t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));              // eq.19
          t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));              // eq.19
          t2 = t0 % t1;                                                       // c= a x b // eq.20
      
          if (!renorm(t0, _dcm_matrix.a) ||
                  !renorm(t1, _dcm_matrix.b) ||
                  !renorm(t2, _dcm_matrix.c)) {
              // Our solution is blowing up and we will force back
              // to last euler angles
              _last_failure_ms = AP_HAL::millis();
              AP_AHRS_DCM::reset(true);
          }
      }
      
      // renormalise one vector component of the DCM matrix
      // this will return false if renormalization fails
      bool
      AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
      {
          float renorm_val;
      
          // numerical errors will slowly build up over time in DCM,
          // causing inaccuracies. We can keep ahead of those errors
          // using the renormalization technique from the DCM IMU paper
          // (see equations 18 to 21).
      
          // For APM we don't bother with the taylor expansion
          // optimisation from the paper as on our 2560 CPU the cost of
          // the sqrt() is 44 microseconds, and the small time saving of
          // the taylor expansion is not worth the potential of
          // additional error buildup.
      
          // Note that we can get significant renormalisation values
          // when we have a larger delta_t due to a glitch eleswhere in
          // APM, such as a I2c timeout or a set of EEPROM writes. While
          // we would like to avoid these if possible, if it does happen
          // we don't want to compound the error by making DCM less
          // accurate.
      
          renorm_val = 1.0f / a.length(); //这里并没有使用泰勒展开,考虑的节省的时间不多
      
          // keep the average for reporting
          _renorm_val_sum += renorm_val;
          _renorm_val_count++;
      
          if (!(renorm_val < 2.0f && renorm_val > 0.5f)) {
              // this is larger than it should get - log it as a warning
              if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) {
                  // we are getting values which are way out of
                  // range, we will reset the matrix and hope we
                  // can recover our attitude using drift
                  // correction before we hit the ground!
                  //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
                  //     renorm_val);
                  return false;
              }
          }
      
          result = a * renorm_val;
          return true;
      }
      

    EKF设计与实现

    资料搜集

    APM的EKF源码流分析:
    ArduCopter.cpp里fast_loop函数里的姿态更新部分如下:

    void AP_AHRS_NavEKF::update(void)
    {
        update_DCM();
        update_EKF1();
        update_EKF2();
    #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        update_SITL();
    #endif
    }
    

    这里看出姿态估算使用了DCM和EKF算法,而EKF由分为两种,第一种为默认推荐的稳定版代码飞行的,第二种为git上master的,是另外一种形式的升级版EKF,这种EKF可以通过一个移动的平台起飞。下面以master版本分析,具体解释如下:

    void AP_AHRS_NavEKF::update_EKF2(void) //master版(即开发版)实验表明,最后是由这个函数生成的姿态角
    {
        if (!ekf2_started) {
            // wait 1 second for DCM to output a valid tilt error estimate
            if (start_time_ms == 0) {
                start_time_ms = AP_HAL::millis();
            }
            if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
                ekf2_started = EKF2.InitialiseFilter(); //使用了DCM的_error_rp = 0.8f * _error_rp + 0.2f * best_error;,来源drift_correction,所以EKF依赖于DCM
            }
        }
        if (ekf2_started) {
            EKF2.UpdateFilter();
            if (active_EKF_type() == EKF_TYPE2) {
                Vector3f eulers;
                EKF2.getRotationBodyToNED(_dcm_matrix); 这里的矩阵重新赋值了DCM矩阵,即姿态
                EKF2.getEulerAngles(-1,eulers);
                roll  = eulers.x; //生成roll
                pitch = eulers.y;
                yaw   = eulers.z;
    
                update_cd_values(); //生成roll_sensor
                update_trig(); //生成_cos_roll
    
                // keep _gyro_bias for get_gyro_drift()
                EKF2.getGyroBias(-1,_gyro_bias);
                _gyro_bias = -_gyro_bias;
    
                // calculate corrected gryo estimate for get_gyro()
                _gyro_estimate.zero();
                uint8_t healthy_count = 0;
                for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
                    if (_ins.get_gyro_health(i) && healthy_count < 2) {
                        _gyro_estimate += _ins.get_gyro(i);
                        healthy_count++;
                    }
                }
                if (healthy_count > 1) {
                    _gyro_estimate /= healthy_count;
                }
                _gyro_estimate += _gyro_bias;
    
                float abias;
                EKF2.getAccelZBias(-1,abias);
    
                // This EKF uses the primary IMU
                // Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
                // update _accel_ef_ekf
                for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
                    Vector3f accel = _ins.get_accel(i);
                    if (i==_ins.get_primary_accel()) {
                        accel.z -= abias;
                    }
                    if (_ins.get_accel_health(i)) {
                        _accel_ef_ekf[i] = _dcm_matrix * accel;
                    }
                }
                _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
            }
        }
    }
    

    概要:在这个函数里有一个active_EKF_type()函数,通过分析可知它的值为EKF_TYPE2,故使用的是EKF2算法。具体分析如下

    AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::active_EKF_type(void) const
    {
        EKF_TYPE ret = EKF_TYPE_NONE;
    /*
        enum EKF_TYPE {EKF_TYPE_NONE=0,
                       EKF_TYPE1=1,
                       EKF_TYPE2=2
    #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
                       ,EKF_TYPE_SITL=10
    #endif
        };
    */
        switch (ekf_type()) {
        case 0:
            return EKF_TYPE_NONE;
    
        case 1: { 
            // do we have an EKF yet?
            if (!ekf1_started) {
                return EKF_TYPE_NONE;
            }
            if (always_use_EKF()) {
    /*
        bool always_use_EKF() const {
            return _ekf_flags & FLAG_ALWAYS_USE_EKF; //下面分析这两个数据,结果为0
        }
    
    // constructor
    AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
                                   NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags) :
        AP_AHRS_DCM(ins, baro, gps),
        EKF1(_EKF1),
        EKF2(_EKF2),
        _ekf_flags(flags)
    {
        _dcm_matrix.identity();
    }
    
    enum Flags {
            FLAG_NONE = 0,
            FLAG_ALWAYS_USE_EKF = 0x1, //FLAG_ALWAYS_USE_EKF = 0x1
        };
    
        // Constructor
        AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng,
                       NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE); //所以_ekf_flags = FLAG_NONE = 0
    */
                uint8_t ekf_faults;
                EKF1.getFilterFaults(ekf_faults);
                if (ekf_faults == 0) {
                    ret = EKF_TYPE1;
                }
            } else if (EKF1.healthy()) {
                ret = EKF_TYPE1;
            }
            break;
        }
    
        case 2: { //由下面的函数分析可知,应该会跳到这里
            // do we have an EKF2 yet?
            if (!ekf2_started) {
                return EKF_TYPE_NONE;
            }
            if (always_use_EKF()) {
                uint8_t ekf2_faults;
                EKF2.getFilterFaults(-1,ekf2_faults);
                if (ekf2_faults == 0) {
                    ret = EKF_TYPE2;
                }
            } else if (EKF2.healthy()) {
                ret = EKF_TYPE2; //最终的返回结果
            }
            break;
        }
    
    #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        case EKF_TYPE_SITL:
            ret = EKF_TYPE_SITL;
            break;
    #endif
        }
    
        /*
          fixed wing and rover when in fly_forward mode will fall back to
          DCM if the EKF doesn't have GPS. This is the safest option as
          DCM is very robust
         */
        if (ret != EKF_TYPE_NONE &&
            (_vehicle_class == AHRS_VEHICLE_FIXED_WING ||
             _vehicle_class == AHRS_VEHICLE_GROUND) &&
            _flags.fly_forward) {
            nav_filter_status filt_state;
            if (ret == EKF_TYPE1) {
                EKF1.getFilterStatus(filt_state);
    #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
            } else if (ret == EKF_TYPE_SITL) {
                get_filter_status(filt_state);
    #endif
            } else {
                EKF2.getFilterStatus(-1,filt_state);
            }
            if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
                // if the EKF is not fusing GPS and we have a 3D lock, then
                // plane and rover would prefer to use the GPS position from
                // DCM. This is a safety net while some issues with the EKF
                // get sorted out
                return EKF_TYPE_NONE;
            }
            if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {
                return EKF_TYPE_NONE;
            }
            if (!filt_state.flags.attitude ||
                    !filt_state.flags.horiz_vel ||
                    !filt_state.flags.vert_vel ||
                    !filt_state.flags.horiz_pos_abs ||
                    !filt_state.flags.vert_pos) {
                return EKF_TYPE_NONE;
            }
        }
        return ret;
    }
    

    其中又涉及到了ekf_type函数,可知返回值为2:

    //canonicalise _ekf_type, forcing it to be 0, 1 or 2
    
    uint8_t AP_AHRS_NavEKF::ekf_type(void) const
    {
        uint8_t type = _ekf_type; //由下面的分析,默认_ekf_type的值为2
        if (always_use_EKF() && type == 0) { //如果总是使用EKF且默认type为0时,那么type就强制为1
            type = 1;
        }
    
    //  bool always_use_EKF() const {
    //      return _ekf_flags & FLAG_ALWAYS_USE_EKF; //位运算,结果为0
    //  }
    
        // check for invalid type
    #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        if (type > 2 && type != EKF_TYPE_SITL) { //检查type是否有效,只有为SIL的时候才能大于2
            type = 1;
        }
    #else
        if (type > 2) {
            type = 1;
        }
    #endif
        return type;
    }
    

    _ekf_type的默认取值分析。

    #if AP_AHRS_NAVEKF_AVAILABLE //_ekf_type的默认取值为2
        // @Param: EKF_TYPE
        // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
        // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation and whether fallback to the DCM algorithm is allowed. Note that on copters "disabled" is not available, and will be the same as "enabled - no fallback"
        // @Values: 0:Disabled,1:Enabled,2:Enable EKF2
        // @User: Advanced
        AP_GROUPINFO("EKF_TYPE",  14, AP_AHRS, _ekf_type, 2),
    #endif
    

    在DCM算法里,与ekf算法一样,最后都生成了roll, roll_sensor, _cos_roll。

    // calculate the euler angles and DCM matrix which will be used for high level
    // navigation control. Apply trim such that a positive trim value results in a
    // positive vehicle rotation about that axis (ie a negative offset)
    void
    AP_AHRS_DCM::euler_angles(void)
    {
        _body_dcm_matrix = _dcm_matrix;
        _body_dcm_matrix.rotateXYinv(_trim);
        _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
    
        update_cd_values();
    }
    
    /*
      update the centi-degree values
     */
    void AP_AHRS::update_cd_values(void)
    {
        roll_sensor  = degrees(roll) * 100;
        pitch_sensor = degrees(pitch) * 100;
        yaw_sensor   = degrees(yaw) * 100;
        if (yaw_sensor < 0)
            yaw_sensor += 36000;
    }
    
    // update trig values including _cos_roll, cos_pitch
    update_trig();
    

    EKF1与EKF2的切换:这里假设你自己在开发版代码中想使用EKF1怎么办?
    直接进行参数配置或者在地面站修改EKF相关的参数即可,具体如下修改即可

    1. 源码修改,全局搜索到如下地方,更改成下面的样子。

       AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 0, AP_PARAM_FLAG_ENABLE),
       AP_GROUPINFO_FLAGS("ENABLE", 34, NavEKF, _enable, 1, AP_PARAM_FLAG_ENABLE),
       AP_GROUPINFO("EKF_TYPE",  14, AP_AHRS, _ekf_type, 1),
      
    2. 地面站也是要修改三个地方。

    可以总结出各个参数都在各自的cpp文件中有默认值
    如AP_MotorsMulticopter.cpp中的AP_GROUPINFO("CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),设置解锁时电机的转速。


    姿态控制

    预习材料:PID参数调节/串级PID/串级PID1

    下面将先进行APM源码自稳模式的PID数据流分析:
    在AC_AttitudeControl.cpp里

    void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
    {
        // Update euler attitude target and angular velocity target
        att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z); //将四元数的目标姿态角装换为欧拉角
        _att_target_ang_vel_rads = att_target_ang_vel_rads;
    
        // Retrieve quaternion vehicle attitude
        // TODO add _ahrs.get_quaternion()
        Quaternion att_vehicle_quat;
        att_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned()); //获得机体姿态角
        // Compute attitude error
        (att_vehicle_quat.inverse()*att_target_quat).to_axis_angle(_att_error_rot_vec_rad); //计算角度误差
    
        // Compute the angular velocity target from the attitude error
        update_ang_vel_target_from_att_error(); //内环P控制,更新_ang_vel_target_rads外环控制值
    
        // Add the angular velocity feedforward, rotated into vehicle frame
        Matrix3f Trv;
        get_rotation_reference_to_vehicle(Trv);
        _ang_vel_target_rads += Trv * _att_target_ang_vel_rads; //更新_ang_vel_target_rads外环控制值
    }
    

    其中get_rotation_body_to_ned函数的原型为:

    const Matrix3f &AP_AHRS_NavEKF::get_rotation_body_to_ned(void) const
    {
        if (!active_EKF_type()) { //如果没有使用EKF,则使用DCM算法生成的矩阵
            return AP_AHRS_DCM::get_rotation_body_to_ned();
        }
        return _dcm_matrix; //否则使用EKF2生成的矩阵
    }
    
    // return rotation matrix representing rotaton from body to earth axes
    onst Matrix3f &get_rotation_body_to_ned(void) const {
        return _body_dcm_matrix;
    }
    

    另为,内环P控制,输出值_ang_vel_target_rads为外环PID目标控制值。

    void AC_AttitudeControl::update_ang_vel_target_from_att_error()
    {
        // Compute the roll angular velocity demand from the roll angle error
        if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) {
            _ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f,  AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
        }else{
            _ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x;
        }
    
        // Compute the pitch angular velocity demand from the roll angle error
        if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) {
            _ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f,  AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
        }else{
            _ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y;
        }
    
        // Compute the yaw angular velocity demand from the roll angle error
        if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) {
            _ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f,  AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
        }else{
            _ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z;
        }
    
        // Add feedforward term that attempts to ensure that the copter yaws about the reference
        // Z axis, rather than the vehicle body Z axis.
        // NOTE: This is a small-angle approximation.
        _ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z;
        _ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z;
    }
    

    外环PID控制并设置电机值。

    void AC_AttitudeControl::rate_controller_run()
    {
        _motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
        _motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
        _motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
    }
    

    这个函数位于ArduCopter.cpp里的fast_loop函数里,它是放于内环控制之前的。如下:

    // Main loop - 400hz
    void Copter::fast_loop()
    {
    
        // IMU DCM Algorithm
        // --------------------
        read_AHRS();
    
        // run low level rate controllers that only require IMU data
        attitude_control.rate_controller_run(); //外环并赋值给电机
    
    #if FRAME_CONFIG == HELI_FRAME
        update_heli_control_dynamics();
    #endif //HELI_FRAME
    
        // send outputs to the motors library
        motors_output();
    
        // Inertial Nav
        // --------------------
        read_inertia();
    
        // check if ekf has reset target heading
        check_ekf_yaw_reset();
    
        // run the attitude controllers
        update_flight_mode(); //如果仅考虑自稳模式,内环控制在这个里边
    
        // update home from EKF if necessary
        update_home_from_EKF();
    
        // check if we've landed or crashed
        update_land_and_crash_detectors();
    
    #if MOUNT == ENABLED
        // camera mount's fast update
        camera_mount.update_fast();
    #endif
    
        // log sensor health
        if (should_log(MASK_LOG_ANY)) {
            Log_Sensor_Health();
        }
    }
    

    看看rate_controller_run函数其中的rate_bf_to_motor_roll函数,这里进行了外环控制。

    float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
    {
        float current_rate_rads = _ahrs.get_gyro().x;
        float rate_error_rads = rate_target_rads - current_rate_rads;
    
        // For legacy reasons, we convert to centi-degrees before inputting to the PID
        _pid_rate_roll.set_input_filter_d(degrees(rate_error_rads)*100.0f);
        _pid_rate_roll.set_desired_rate(degrees(rate_target_rads)*100.0f);
    
        float integrator = _pid_rate_roll.get_integrator();
    
        // Ensure that integrator can only be reduced if the output is saturated
        if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) {
            integrator = _pid_rate_roll.get_i();
        }
    
        // Compute output
        float output = _pid_rate_roll.get_p() + integrator + _pid_rate_roll.get_d();
    
        // Constrain output
        return constrain_float(output, -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
    }
    

    串口、GCS及LOG调试

    由于我们在姿态算法测试的过程当中需要及时查看相关数据的变化情况或者波形图,在APM里主要有几种方法可以实现我们的目的。
    第一种为串口:这种方式不仅在姿态算法测试而且在系统调试的过程当中都很起作用,假如你的Pixhawk突然启动出错,这个时候你就可以通过串口查看板子上电后rcS(输出信息都使用echo)的启动过程。另一方面,在APM的代码里,有独立的库文件可以进行编译学习,目的是可以让你单独快速的学习这个模块,如下,而这个里边的输出信息直接打印到了串口。

    void setup()
    {
        hal.console->println("OpticalFlow library test ver 1.6");
    
        hal.scheduler->delay(1000);
    
        // flowSensor initialization
        optflow.init();
    
        if (!optflow.healthy()) {
            hal.console->print("Failed to initialise PX4Flow ");
        }
    
        hal.scheduler->delay(1000);
    }
    
    void loop()
    {
        hal.console->println("this only tests compilation succeeds");
    
        hal.scheduler->delay(5000);
    }
    
    AP_HAL_MAIN
    

    串口的连接方式如下:

    第二种方式为GCS:即通过飞控与地面站建立通信,在地面站上显示图形界面来显示波形。下面结合源码简单分析一下GCS:
    从Arducopter.cpp可以看出,与地面站通信的主要线程为:

        SCHED_TASK(gcs_check_input,      400,    180),
        SCHED_TASK(gcs_send_heartbeat,     1,    110),
        SCHED_TASK(gcs_send_deferred,     50,    550),
        SCHED_TASK(gcs_data_stream_send,  50,    550),
    

    但是有一个地方需要注意,在void Copter::init_ardupilot()函数中,有一个设置:

        // Register the mavlink service callback. This will run
        // anytime there are more than 5ms remaining in a call to
        // hal.scheduler->delay.
        hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
    

    而这个函数为mavlink_delay_cb_static,追踪进去copter.mavlink_delay_cb();,再进去就是:

    /*
     *  a delay() callback that processes MAVLink packets. We set this as the
     *  callback in long running library initialisation routines to allow
     *  MAVLink to process packets while waiting for the initialisation to
     *  complete
     */
    void Copter::mavlink_delay_cb()
    {
        static uint32_t last_1hz, last_50hz, last_5s;
        if (!gcs[0].initialised || in_mavlink_delay) return;
    
        in_mavlink_delay = true;
    
        uint32_t tnow = millis();
        if (tnow - last_1hz > 1000) {
            last_1hz = tnow;
            gcs_send_heartbeat();
            gcs_send_message(MSG_EXTENDED_STATUS1);
        }
        if (tnow - last_50hz > 20) {
            last_50hz = tnow;
            gcs_check_input();
            gcs_data_stream_send();
            gcs_send_deferred();
            notify.update();
        }
        if (tnow - last_5s > 5000) {
            last_5s = tnow;
            gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM");
        }
        check_usb_mux();
    
        in_mavlink_delay = false;
    }
    

    可知在这里也进行了数据的发送,那岂不是与之前的线程冲突了?
    仔细查看hal.scheduler->register_delay_callback这个函数:

    void PX4Scheduler::register_delay_callback(AP_HAL::Proc proc,
                                                uint16_t min_time_ms) 
    {
        _delay_cb = proc;
        _min_delay_cb_ms = min_time_ms;
    }
    

    而_delay_cb,_min_delay_cb_ms在delay函数里得到了调用:

    void PX4Scheduler::delay(uint16_t ms)
    {
        if (in_timerprocess()) {
            ::printf("ERROR: delay() from timer process\n");
            return;
        }
        perf_begin(_perf_delay);
        uint64_t start = AP_HAL::micros64();
    
        while ((AP_HAL::micros64() - start)/1000 < ms && 
               !_px4_thread_should_exit) {
            delay_microseconds_semaphore(1000);
            if (_min_delay_cb_ms <= ms) {
                if (_delay_cb) {
                    _delay_cb();
                }
            }
        }
        perf_end(_perf_delay);
        if (_px4_thread_should_exit) {
            exit(1);
        }
    }
    

    因此可以知道这里的意思就是,如果系统有延时过长(_min_delay_cb_ms <= ms)的情况,则会给gcs发送数据,这样以防止与gcs通信中断。

    主要发送消息流函数gcs_data_stream_send

    /*
     *  send data streams in the given rate range on both links
     */
    void Copter::gcs_data_stream_send(void)
    {
        for (uint8_t i=0; i<num_gcs; i++) {
            if (gcs[i].initialised) {
                gcs[i].data_stream_send();
            }
        }
    }
    

    如果我们继续往里边看,可以看到这里分类进行输出。

    void
    GCS_MAVLINK::data_stream_send(void)
    {
        if (waypoint_receiving) {
            // don't interfere with mission transfer
            return;
        }
    
        if (!copter.in_mavlink_delay && !copter.motors.armed()) {
            handle_log_send(copter.DataFlash);
        }
    
        copter.gcs_out_of_time = false;
    
        if (_queued_parameter != NULL) {
            if (streamRates[STREAM_PARAMS].get() <= 0) {
                streamRates[STREAM_PARAMS].set(10);
            }
            if (stream_trigger(STREAM_PARAMS)) {
                send_message(MSG_NEXT_PARAM);
            }
            // don't send anything else at the same time as parameters
            return;
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (copter.in_mavlink_delay) {
            // don't send any other stream types while in the delay callback
            return;
        }
    
        if (stream_trigger(STREAM_RAW_SENSORS)) {
            send_message(MSG_RAW_IMU1);
            send_message(MSG_RAW_IMU2);
            send_message(MSG_RAW_IMU3);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_EXTENDED_STATUS)) {
            send_message(MSG_EXTENDED_STATUS1);
            send_message(MSG_EXTENDED_STATUS2);
            send_message(MSG_CURRENT_WAYPOINT);
            send_message(MSG_GPS_RAW);
            send_message(MSG_NAV_CONTROLLER_OUTPUT);
            send_message(MSG_LIMITS_STATUS);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_POSITION)) {
            send_message(MSG_LOCATION);
            send_message(MSG_LOCAL_POSITION);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_RAW_CONTROLLER)) {
            send_message(MSG_SERVO_OUT);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_RC_CHANNELS)) {
            send_message(MSG_RADIO_OUT);
            send_message(MSG_RADIO_IN);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_EXTRA1)) {
            send_message(MSG_ATTITUDE);
            send_message(MSG_SIMSTATE);
            send_message(MSG_PID_TUNING);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_EXTRA2)) {
            send_message(MSG_VFR_HUD);
        }
    
        if (copter.gcs_out_of_time) return;
    
        if (stream_trigger(STREAM_EXTRA3)) {
            send_message(MSG_AHRS);
            send_message(MSG_HWSTATUS);
            send_message(MSG_SYSTEM_TIME);
            send_message(MSG_RANGEFINDER);
    #if AP_TERRAIN_AVAILABLE && AC_TERRAIN
            send_message(MSG_TERRAIN);
    #endif
            send_message(MSG_BATTERY2);
            send_message(MSG_MOUNT_STATUS);
            send_message(MSG_OPTICAL_FLOW);
            send_message(MSG_GIMBAL_REPORT);
            send_message(MSG_MAG_CAL_REPORT);
            send_message(MSG_MAG_CAL_PROGRESS);
            send_message(MSG_EKF_STATUS_REPORT);
            send_message(MSG_VIBRATION);
            send_message(MSG_RPM);
        }
    }
    

    这么多消息,它的管理方式为队列。

    // send a message using mavlink, handling message queueing
    void GCS_MAVLINK::send_message(enum ap_message id)
    {
        uint8_t i, nextid;
    
        // see if we can send the deferred messages, if any
        while (num_deferred_messages != 0) {
            if (!try_send_message(deferred_messages[next_deferred_message])) {
                break;
            }
            next_deferred_message++;
            if (next_deferred_message == MSG_RETRY_DEFERRED) {
                next_deferred_message = 0;
            }
            num_deferred_messages--;
        }
    
        if (id == MSG_RETRY_DEFERRED) {
            return;
        }
    
        // this message id might already be deferred
        for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++) {
            if (deferred_messages[nextid] == id) {
                // its already deferred, discard
                return;
            }
            nextid++;
            if (nextid == MSG_RETRY_DEFERRED) {
                nextid = 0;
            }
        }
    
        if (num_deferred_messages != 0 ||
            !try_send_message(id)) {
            // can't send it now, so defer it
            if (num_deferred_messages == MSG_RETRY_DEFERRED) {
                // the defer buffer is full, discard
                return;
            }
            nextid = next_deferred_message + num_deferred_messages;
            if (nextid >= MSG_RETRY_DEFERRED) {
                nextid -= MSG_RETRY_DEFERRED;
            }
            deferred_messages[nextid] = id;
            num_deferred_messages++;
        }
    }
    

    注意这里边有个所谓的get_secondary_attitude函数,它是另外一种算法的姿态信息,不是DCM就是EKF:

    // return secondary attitude solution if available, as eulers in radians
    bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
    {
        switch (active_EKF_type()) {
        case EKF_TYPE_NONE:
            // EKF is secondary
            EKF1.getEulerAngles(eulers); //这个是EKF1算法生成的姿态信息
            return ekf1_started;
    
        case EKF_TYPE1:
        case EKF_TYPE2:
        default:
            // DCM is secondary
            eulers = _dcm_attitude; //这是DCM算法生成的姿态信息,应该是返回这里
            return true;
        }
    }
    

    消息由mav_finalize_message_chan_send定制,最后由comm_send_buffer发出~
    如果飞控出现问题如,gcs
    send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks");,是这个函数发出来的。

    在apmplanner里查看波形如下:

    第三种当然是通过SD卡咯,一般来说,当我们实际飞行完成后,可以将SD里面的飞行数据进行分析,对于APM的固件,数据打包成bin格式的文件存储于SD卡中,可以使用apmplanner(如上图中的OpenLog)读取,也可以通过MAVExplorer。实际操作很简单,不进行分析。
    APM的源码体现为:

    // fifty_hz_logging_loop
    // should be run at 50hz
    void Copter::fifty_hz_logging_loop()
    {
    #if HIL_MODE != HIL_MODE_DISABLED
        // HIL for a copter needs very fast update of the servo values
        gcs_send_message(MSG_RADIO_OUT);
    #endif
    
    #if HIL_MODE == HIL_MODE_DISABLED
        if (should_log(MASK_LOG_ATTITUDE_FAST)) {
            Log_Write_Attitude();
            Log_Write_Rate();
            if (should_log(MASK_LOG_PID)) {
                DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
                DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
                DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
                DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
            }
        }
    
        // log IMU data if we're not already logging at the higher rate
        if (should_log(MASK_LOG_IMU) && !(should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW))) {
            DataFlash.Log_Write_IMU(ins);
        }
    #endif
    }
    
    展开全文
  • 飞控算法】四旋翼飞行器控制原理与设计入门

    千次阅读 多人点赞 2019-02-12 14:21:54
    从动力学建模和几个四旋翼核心算法角度分析半自主飞控系统的建立,即实现传统四旋翼的姿态控制和高度控制的过程,文章主要借鉴了北航多旋翼设计课程、正点原子minifly微型四旋翼的资料、《四旋翼无人飞行器设计》...

    从动力学建模和几个四旋翼核心算法角度分析半自主飞控系统的建立,即实现传统四旋翼的姿态控制和高度控制的过程,文章主要借鉴了北航多旋翼设计课程、正点原子minifly微型四旋翼的资料、《四旋翼无人飞行器设计》清华出版社,代码示例来自正点原子。但是吧,后两个资料在理论部分都有点东拼西凑的感觉,有些错误,看起来很伤…总之,本文多处为个人理解,如有错误,感谢指出。

    目录

    一、相关理论知识

    1.坐标系与欧拉角

    2.旋转矩阵

    3.四元数及其与欧拉角的关系

    二、控制模型建立

    1.四旋翼动力学模型

    2.简化为控制模型

    3.四旋翼控制分配模型

    三、控制算法及实现

    1.姿态解算

    2.串级PID控制

    3.控制分配


    一、相关理论知识

    1.坐标系与欧拉角

    进行动力学建模之前首先建立坐标系,在此建立地球坐标系O_{e}-x_{e}y_{e}z_{e}和机体坐标系O_{b}-x_{b}y_{b}z_{b},如图所示,这里地球系z轴方向向下指向地心,机体系x轴为机头方向。

    当描述一个三维空间内的刚体转动时,需要选用三个独立的角度来表示刚体的相对位置。即,刚体绕固定点的旋转可以看成是若干次旋转的合成,旋转方法不是唯一的,所以欧拉角有多种取法,不同的取法、不同的转动顺序都会对于不同的旋转矩阵。在研究四旋翼时,为了与四旋翼的滚转、俯仰、偏航相对应,可以取绕x旋转角度为φ(滚转,右滚为正),绕y轴旋转角度为θ(俯仰,仰头为正),绕z轴旋转为ψ(偏航,右偏为正)。

    由地球坐标系转到机体坐标系为xyz旋转,又称卡尔丹角。实际上理论分析时,旋转顺序不是很重要,zyx、zxy都可以,虽然最后姿态解算时四元数与欧拉角的关系式不同,但是都可以进行解算。

    2.旋转矩阵

    绕x轴旋转的旋转矩阵为,

    C_{x}(\phi )=\begin{bmatrix} 1 & 0 & 0\\ 0 &cos\phi &sin\phi \\ 0& -sin\phi & cos\phi \end{bmatrix}

    绕y轴旋转的旋转矩阵为,

    C_{y}(\theta )=\begin{bmatrix} \cos \theta & 0&-\sin \theta \\ 0& 1 &0 \\ \sin \theta & 0 &\cos \theta \end{bmatrix}

    绕z轴旋转的旋转矩阵为,

    C_{z}(\psi )=\begin{bmatrix} \cos \psi & \sin \psi &0 \\ -\sin \psi &\cos \psi&0 \\ 0& 0 &1 \end{bmatrix}

    这样,xyz旋转时,一个向量由地球系的表示转为机体系的表示,可以写成,

    \underset{b^{b}}{\rightarrow}=C_{z}( \phi )\cdot C_{y}(\theta )\cdot C_{x}(\phi )\cdot \underset{b^{e}}{\rightarrow}= C_{e}^{b}\cdot \underset{b^{e}}{\rightarrow}

    C_{e}^{b}=\begin{bmatrix} \cos\theta \cos \psi &\sin \phi \sin \theta \cos \psi +\cos \phi \sin \psi & -\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi \\ -\cos \theta \sin \psi & -\sin \phi \sin \theta \sin \psi +\cos \phi \cos \psi & \cos \phi \sin \theta \sin \psi +\sin \phi \cos \psi \\ \sin \theta &- \sin \phi \cos \theta & \cos \phi\cos \theta \end{bmatrix}

    由机体系转向地球系的姿态矩阵为,C_{b}^{e}=(C_{e}^{b})^{-1}=(C_{e}^{b})^{T}

    {C_{x}(\phi )}^{-1}=C_{x}(-\phi )={C_{x}(\phi )}^{T}

    3.四元数及其与欧拉角的关系

    关于四元数的详细定义等参见秦永元的《惯性导航》,非常详细,这里不搬了。

    以zyx旋转顺序为例,此时其旋转矩阵

    C_{e}^{b}=\begin{bmatrix} \cos\theta \cos \psi &\sin \psi \cos \theta &- \sin \theta \\\sin \phi \sin \theta \cos \psi -\cos \phi \sin \psi & \sin \phi \sin \theta \sin \psi +\cos \phi \cos \psi &\sin \phi \cos \theta\\ \cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi & \cos \phi \sin \theta \sin \psi -\sin \phi \cos \psi & \cos \phi\cos \theta \end{bmatrix}

    此时由四元数表示的旋转为

    \begin{bmatrix} x_{b}\\ y_{b} \\ z_{b} \end{bmatrix}=\begin{bmatrix} q_{0}^{2}+ q_{1}^{2}- q_{2}^{2}- q_{3}^{2}&2(q_{1}q_{2}+q_{0}q_{3}) &2(q_{1}q_{3}-q_{0}q_{2}) \\ 2(q_{1}q_{2}-q_{0}q_{3}) &q_{0}^{2}- q_{1}^{2}+q_{2}^{2}- q_{3}^{2} &2(q_{2}q_{3}+q_{0}q_{1}) \\ 2(q_{1}q_{3}+q_{0}q_{2}) & 2(q_{2}q_{3}-q_{0}q_{1}) & q_{0}^{2}-q_{1}^{2}- q_{2}^{2}+q_{3}^{2} \end{bmatrix}\begin{bmatrix} x_{e}\\ y_{e} \\ z_{e} \end{bmatrix}

    这样一一对应,可以求得后面姿态解算时要用到的一些式子,

    ①重力在机体坐标系下的表示:

    \begin{bmatrix} Vx_{b}\\ Vy_{b} \\ Vz_{b} \end{bmatrix}\\=\begin{bmatrix} q_{0}^{2}+ q_{1}^{2}- q_{2}^{2}- q_{3}^{2}&2(q_{1}q_{2}+q_{0}q_{3}) &2(q_{1}q_{3}-q_{0}q_{2}) \\ 2(q_{1}q_{2}-q_{0}q_{3}) &q_{0}^{2}- q_{1}^{2}+q_{2}^{2}- q_{3}^{2} &2(q_{2}q_{3}+q_{0}q_{1}) \\ 2(q_{1}q_{3}+q_{0}q_{2}) & 2(q_{2}q_{3}-q_{0}q_{1}) & q_{0}^{2}-q_{1}^{2}- q_{2}^{2}+q_{3}^{2} \end{bmatrix}\begin{bmatrix} 0\\0\\1\end{bmatrix} \\=\begin{bmatrix} 2(q_{1}q_{3}-q_{0}q_{2})\\ 2(q_{2}q_{3}+q_{0}q_{1}) \\ q_{0}^{2}-q_{1}^{2}- q_{2}^{2}+q_{3}^{2} \end{bmatrix}

    ②四元数与欧拉角的关系:

    \phi =\arctan \frac{2(q_{2}q_{3}+q_{0}q_{1})}{q_{0}^{2}-q_{1}^{2}- q_{2}^{2}+q_{3}^{2}}

    \theta =-\arcsin 2(q_{1}q_{3}-q_{0}q_{2})

    \psi =\arctan \frac{2(q_{1}q_{2}+q_{0}q_{3})}{q_{0}^{2}+q_{1}^{2}- q_{2}^{2}-q_{3}^{2} }

    四元数具体的推导和计算太多了,详见《捷联式惯性导航原理》。关于四元数与旋转矩阵我查阅了很多资料,比较官方的书呢,推导都是用的zyx旋转顺序,推出上式中四元数与欧拉角的关系。其他资料呢,有用不同的旋转顺序,但是对应旋转矩阵时很容易搞错。我觉得四元数与欧拉角的关系不是唯一的,只取决于使用的四元数,旋转顺序不同,对应的四元数自然不同。这主要影响的是解算过程中,q的值可能是不同的,但解算出的欧拉角都是一样的。

    二、控制模型建立

    首先,建模的目的是基于这个模型设计控制器。四旋翼是一个非线性的多输入系统,但是,为了简化处理,在小角度时,可以将模型简化为线性的,进而就可以使用线性控制的方法处理。

    1.四旋翼动力学模型

    ①欧拉角表示为,

    \dot{p}_{e}=v_{e}

    \dot{v}_{e}=ge_{3}-(\tfrac{f}{m})R\cdot e_{3}

    \dot{\Theta }=W\cdot \omega _{b}

    J\cdot\dot{\omega _{b}}=-\omega _{b}\times (J\times \omega _{b})+G_{a}+\tau

    其中,W=\frac{1}{\cos \theta }\begin{bmatrix} \cos \theta &\sin \phi \sin \theta &\cos \phi \sin \theta \\ 0&\cos \phi \cos \theta &-\sin \phi \cos \theta \\ 0 & \sin \phi &\cos \phi \end{bmatrix}

    ②四元数表示为,

    \dot{p}_{e}=v_{e}

    \dot{v}_{e}=ge_{3}-(\tfrac{f}{m})R\cdot e_{3}

    \dot{q_{0}}=-\frac{1}{2}{q_{v}}^{T}\cdot \omega _{b}

    \dot{q_{v}}=\frac{1}{2}(q_{0}\cdot I_{3}+\begin{bmatrix} q_{v} \end{bmatrix}_{\times })\cdot \omega _{b}

    (上面两个式子实际上是四元数运动学微分方程,后面姿态解算时会用到)

    J\cdot\dot{\omega _{b}}=-\omega _{b}\times (J\times \omega _{b})+G_{a}+\tau

    2.简化为控制模型

    在这里研究给定期望姿态角,期望高度的控制模型,这样可以分为四个通道:高度通道、俯仰通道、滚转通道、偏航通道,俯仰通道、滚转通道、偏航通道模型是一样的,是姿态模型。

    ①先看高度通道(z方向)的化简,

    \dot{p}_{e}=v_{e}

    \dot{v}_{e}=ge_{3}-(\tfrac{f}{m})R\cdot e_{3}

    变为,

    \dot{p}_{z}=v_{z}

    \dot{v}_{z}=g-\tfrac{f}{m}(俯仰、滚转都很小时)

    ②再看姿态通道的化简,

    \dot{\Theta }=W\cdot \omega _{b}

    J\cdot\dot{\omega _{b}}=-\omega _{b}\times (J\times \omega _{b})+G_{a}+\tau

    变为,

    \dot{\Theta }= \omega _{b}

    J\cdot\dot{\omega _{b}}=\tau(角度小,速度小时)

    3.四旋翼控制分配模型

    控制分配模型是建立伪控制量与实际控制量之间关系的模型,以X4型传统四旋翼为例,其伪控制量实际是几个通道经过pid之后的输出值pidout,高度通道的输出可以看做是总拉力,.姿态通道可以看成是力矩(具体分析就涉及到pid控制器的设计了);其实际控制量可以看成是电机转速的平方。这几个伪控制量与实际控制量的对应关系如下:

    \begin{bmatrix} f\\ \tau _{x} \\ \tau _{y} \\ \tau _{z} \end{bmatrix}= \begin{bmatrix} c_{T} & c_{T} &c_{T} &c_{T} \\ \frac{\sqrt{2}}{2}dc_{T} &-\frac{\sqrt{2}}{2}dc_{T} &-\frac{\sqrt{2}}{2}dc_{T} & \frac{\sqrt{2}}{2}dc_{T} \\ \frac{\sqrt{2}}{2}dc_{T} &\frac{\sqrt{2}}{2}dc_{T} &-\frac{\sqrt{2}}{2}dc_{T} & -\frac{\sqrt{2}}{2}dc_{T} \\ c_{M} & c_{M} &c_{M} & c_{M} \end{bmatrix}\begin{bmatrix} {\varpi _{1}}^{2}\\ {\varpi _{2}}^{2} \\ {\varpi _{3}}^{2} \\ {\varpi _{4}}^{2} \end{bmatrix}   ,其中c_{T}是电机转速平方与拉力之间的关系系数,c_{M}是电机转速平方与反扭力之间的关系系数,d是机臂长度。

    控制分配模型在普通四旋翼上的分配基本是固定的,但是当伪控制量与实际控制量个数不一致时,控制分配就很重要。

    三、控制算法及实现

    全自主飞控的控制逻辑是从给定期望轨迹和期望偏航开始的,首先经过这一位置控制得到期望的拉力和期望的三个姿态角,在这里,我暂不考虑轨迹控制,即飞控的逻辑是从给定期望姿态角和期望高度开始。

    这样,飞控底层框架大致如下图,

    图比较乱,硬件、算法、数据都在里面了,基本能看懂…可以看出,四旋翼的三个核心算法:姿态解算算法、pid控制算法、控制分配算法。

    1.姿态解算

    四旋翼姿态解算的方法有很多,有基于四元数,有基于旋转矩阵,有互补滤波,有卡尔曼滤波,其实不管是哪种滤波解算的算法,思想都是利用加速度计解算出得数据去修正陀螺仪积分产生偏差。在这里我主要研究了一下最常用的,四元数互补滤波,这个资料已经比较全了,每一个步骤我只记录一下我的理解。

    (1)初始化四元数

    static float q0 = 1.0f;	
    static float q1 = 0.0f;
    static float q2 = 0.0f;
    static float q3 = 0.0f;	

    (2)加速度计低通滤波,去除一部分高频噪声,获取加速度计、陀螺仪值

    (3)加速度计测量值规范化

    normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
    acc.x *= normalise;
    acc.y *= normalise;
    acc.z *= normalise;

    (4)提取四元数的等效余弦矩阵中的重力分量

    vecxZ = 2 * (q1 * q3 - q0 * q2);
    vecyZ = 2 * (q0 * q1 + q2 * q3);
    veczZ = q0s - q1s - q2s + q3s;	

    (5)向量叉积得出偏差

    ex = (acc.y * veczZ - acc.z * vecyZ);
    ey = (acc.z * vecxZ - acc.x * veczZ);
    ez = (acc.x * vecyZ - acc.y * vecxZ);

    (6)PI修正陀螺仪

    exInt += Ki * ex * dt ;  
    eyInt += Ki * ey * dt ;
    ezInt += Ki * ez * dt ;
    gyro.x += Kp * ex + exInt;
    gyro.y += Kp * ey + eyInt;
    gyro.z += Kp * ez + ezInt;

    解释一下(5)、(6),首先在(3)、(4)中保证了加速度计表示的重力分量和由四元数推算出的重力分量都是单位的,这样(5)中的偏差实际上可以理解为角度,且都是在机体坐标系中,我们知道,陀螺仪虽然噪音小,但是其值会有漂移,如果通过积分计算角度,随着时间,其值会累积,这样就得不到准确值了,越偏越多,而加速度计没有漂移,但是噪音很大,且测量值稳定后,即为实际的真值。这样就考虑用加速度计去修正陀螺仪的偏差。实际上,个人理解,这里的姿态误差就可以看成是输入pi控制器的ref-fdb,而通过pi修正之后,得出的out,就可以用了消除陀螺仪的漂移。那么,既然加计是参考值,为什么不能直接置信于加计呢?这里就扯到互补的思想了,加计并不是稳定的,是一直震荡的,不平滑,但值是没有漂移的,积分不会产生大的误差,这样也可以看做是,我们取了陀螺仪稳定的优点,又取了加计没有漂移的优点。

    (7)更新四元数,并归一化

    q0 += (-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z) * halfT;
    q1 += (q0 * gyro.x + q2 * gyro.z - q3 * gyro.y) * halfT;
    q2 += (q0 * gyro.y - q1 * gyro.z + q3 * gyro.x) * halfT;
    q3 += (q0 * gyro.z + q1 * gyro.y - q2 * gyro.x) * halfT;
    
    normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= normalise;
    q1 *= normalise;
    q2 *= normalise;
    q3 *= normalise;

    这里在解释一下四元数更新的算法,这个实际上就是对四元数运动学方程套用龙格库塔方法,这里的计算过程就直接搬运了,没什么好说的。

    (8)解算欧拉角

    这里就是之前说的欧拉角与四元数的对应关系。

    state->attitude.pitch = -asinf(vecxZ) * RAD2DEG; 
    state->attitude.roll = atan2f(vecyZ, veczZ) * RAD2DEG;
    state->attitude.yaw = atan2f(2 * (q1 * q2 + q0 * q3),q0s + q1s - q2s - q3s) * RAD2DEG;
    

    2.串级PID控制

    PID控制器的设计是基于控制模型的,在前面说过,我们设计的PID控制如下,

    ①姿态环

    \large \\ e_{\Theta }= \Theta _{f}-\Theta _{r} \\ \omega_{d}=K_{\Theta p}e_{\Theta }+K_{\Theta i}\int e_{\Theta }+K_{\Theta d}\dot{e_{\Theta }} \\e_{\omega }=\omega -\omega _{d}\\ \tau _{d}=K_{\omega p}e_{\omega }+K_{\omega i}\int e_{\omega }+K_{\omega d}\dot{e}_{\omega }

    void attitudeRatePID(Axis3f *actualRate,attitude_t *desiredRate,control_t *output)	
    {
    	output->roll = pidOutLimit(pidUpdate(&pidRateRoll, desiredRate->roll - actualRate->x));
    	output->pitch = pidOutLimit(pidUpdate(&pidRatePitch, desiredRate->pitch - actualRate->y));
    	output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, desiredRate->yaw - actualRate->z));
    }
    
    void attitudeAnglePID(attitude_t *actualAngle,attitude_t *desiredAngle,attitude_t *outDesiredRate)	
    {
    	outDesiredRate->roll = pidUpdate(&pidAngleRoll, desiredAngle->roll - actualAngle->roll);
    	outDesiredRate->pitch = pidUpdate(&pidAnglePitch, desiredAngle->pitch - actualAngle->pitch);
    
    	float yawError = desiredAngle->yaw - actualAngle->yaw ;
    	if (yawError > 180.0f) 
    		yawError -= 360.0f;
    	else if (yawError < -180.0) 
    		yawError += 360.0f;
    	outDesiredRate->yaw = pidUpdate(&pidAngleYaw, yawError);
    }
    

    姿态环,代码实现的时候还要加入饱和,另外,遥控器模式时,实际是控制yaw的的角速率,直接用单环速率控制。

    ②高度环

    \large \\ e_{P_{z} }= P_{zf}-P _{zr} \\v_{z}_{d}=K_{P_{z}}e_{P_{z}}\\e_{v_{z}}=v_{z} -v_{z}_{d}\\ f _{d}=K_{v_{z} p}e_{v_{z} }+K_{v_{z} i}\int e_{v_{z} }+K_{\omega d}\dot{e}_{v_{z} }

    void altholdPID(float* thrust, const state_t *state, const setpoint_t *setpoint)
    {
    	float newThrust = 0.0;	
    	
    	newThrust = THRUST_SCALE * runPidZ(&posPid.pidVZ, state->position.z, setpoint, POS_UPDATE_DT);
    	
    	if(getCommanderKeyFlight())
    		detecWeight(*thrust, newThrust, state->velocity.z);
    	
    	*thrust = newThrust + posPid.thrustBase;
    	
    	if (*thrust > 60000) 
    	{
    		*thrust = 60000;
    	}	
    }
    
    static float runPidZ(pidAxis_t *axis, float input, const setpoint_t *setpoint, float dt) 
    {
    	float out = 0.f;
    	if (axis->preMode == false && setpoint->isAltHold == true)
    	{
    		positionResetAllPID();
    		axis->setpoint = input + START_HIRHT;
    		posPid.thrustBase = limitThrustBase(configParam.thrustBase);
    	}
    	axis->preMode = setpoint->isAltHold;
    
    	if(setpoint->isAltHold == true)
    	{
    		axis->setpoint += setpoint->velocity.z * dt;
    		out = pidUpdate(&axis->pid, axis->setpoint - input);
    	}
    	return out;
    }

    这里minifly高度环实际也是单环。

    pid更新的代码,

    float pidUpdate(PidObject* pid, const float error)
    {
    	float output;
    
    	pid->error = error;   
    
    	pid->integ += pid->error * pid->dt;
    	if (pid->integ > pid->iLimit)
    	{
    		pid->integ = pid->iLimit;
    	}
    	else if (pid->integ < pid->iLimitLow)
    	{
    		pid->integ = pid->iLimitLow;
    	}
    
    	pid->deriv = (pid->error - pid->prevError) / pid->dt;
    
    	pid->outP = pid->kp * pid->error;
    	pid->outI = pid->ki * pid->integ;
    	pid->outD = pid->kd * pid->deriv;
    
    	output = pid->outP + pid->outI + pid->outD;
    
    	pid->prevError = pid->error;
    
    	return output;
    }

    pid算法虽然都是一样的,但是基于pid控制器,不同飞控实现的方式也不太一样,这里不过多解读minifly的飞控了,后面好好学学px4的源码。

    3.控制分配

    传统四旋翼中,控制分配算法很简单,因为伪控制量和实际控制量的个数是相同的,编号好电机之后,相应正负,分配给电机即可。

    void powerControl(control_t *control)
    {
    	s16 r = control->roll / 2.0f;
    	s16 p = control->pitch / 2.0f;
    	
    	motorPWM.m1 = limitThrust(control->thrust - r - p + control->yaw);
    	motorPWM.m2 = limitThrust(control->thrust - r + p - control->yaw);
    	motorPWM.m3 = limitThrust(control->thrust + r + p + control->yaw);
    	motorPWM.m4 = limitThrust(control->thrust + r - p - control->yaw);	
    }

    传统四旋翼的控制量是四个,在一个三维控制内,自由度有六个,这样的控制就是欠驱动的。如果我的伪控制量有六个,实际控制量也有六个,那我可以实现三维空间内的解耦控制。如果我的伪控制量有六个,实际控制量大于六个,且这六个控制量可以实现对,六个伪控制量做的事情。后面两个种情况控制分配就很重要了,包括六轴啊这种,四个伪控制量,六个实际控制量,也需要合理的控制分配。关于控制分配呢,需要思考的还很多,在这里不多说了。

     

    结语:总之呢,关于四旋翼涉及的面太广了,这里只是把大致框架整理了一下,其实实现四旋翼控制的方法有很多,这里都是最简单的,像水平环的控制,解算时其他一些传感器的融合啊等等都还没说,一些更深入的东西也还没太研究明白,后面继续研究……

    展开全文
  • 飞控算法整理

    2019-07-21 17:46:42
    主要就是飞行器姿态解算,卡尔曼滤波和pid了。。。这些词一直听人说,但就是没人给我讲解过原理。打算自学一下然后整理出来。 途径就是通过查找博客和解析现有...以及pid算法原理和算法流程,和软件实现。 还有姿态...
  • 利用ad公司的芯片adis16470 ,读取六轴传感器的六轴数据,并进行姿态解算。
  • MWC飞控算法详细程序

    千次下载 热门讨论 2011-11-21 17:20:44
    这是从MWC官网下下来的程序,里面有MWC飞控最新的算法程序,很经典,可以借鉴使用!
  • 飞控算法-姿态解算之互补滤波

    千次阅读 2020-04-03 16:24:09
    飞控乱入之姿态解算 姿态解算是通过读取各个传感器的数据进行融合最终得到当前飞行器的姿态,姿态通常用Euler角(Roll-Pitch-Yaw)来表示 姿态解算框架 姿态解算的数据源可来自于陀螺仪或加速度计,他们分别都...
  • 大疆求职算法笔试题 大疆求职算法笔试题大疆求职算法笔试题大疆求职算法笔试题大疆求职算法笔试题大疆求职算法笔试题大疆求职算法笔试题
  • 无人机飞控三大算法

    万次阅读 多人点赞 2018-11-16 15:37:16
    无人机飞控三大算法:捷联式惯性导航系统、卡尔曼滤波算法、飞行控制PID算法。   一、捷联式惯性导航系统 说到导航,不得不说GPS,他是接受卫星发送的信号计算出自身位置的,但是当GPS设备上方被遮挡后,GPS设备...
  • 转载于:https://www.cnblogs.com/yebo92/p/5643519.html
  • 详解几种飞控的姿态解算算法

    万次阅读 多人点赞 2018-05-21 11:16:38
    姿态解算是飞控的一个基础、重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障。有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多。主要是先掌握...
  • 飞控算法之PID

    千次阅读 热门讨论 2015-09-21 21:00:47
    本篇文章中,我们将粗略的了解一下PID过程控制算法,涉及到一下概念和几个分立算法。 PID是什么? 在弄清除PID之前,我们先理解一个概念:调节器。调节器是干什么的?调节器就是人的大脑,就是一个调节系统的核心...
  • 无人机飞控三大算法汇总

    千次阅读 2020-10-18 23:35:45
    无人机飞控三大算法:捷联式惯性导航系统、卡尔曼滤波算法、飞行控制PID算法。 一、捷联式惯性导航系统 说到导航,不得不说GPS,他是接受卫星发送的信号计算出自身位置的,但是当GPS设备上方被遮挡后,GPS设备...
  • 匿名飞控的控制算法

    千次阅读 2020-06-27 06:30:45
    我这里先摘抄一些别人的整理 ...depth_1-utm_source=distribute.pc_relevant_right.none-task-blog-BlogCommendFromMach.
  • hjr-四旋翼飞行器串级PID飞控算法

    千次阅读 多人点赞 2016-03-25 16:33:43
    四旋翼飞行器组件:一个机架,一个陀螺仪,四个无刷直流电机,一个电池,一块单片机(能飞起来的最基本配置)原理:利用四个电机旋转产生的反作用力托起飞行器上升,利用单片机和飞行控制算法控制电机使飞行器稳定然...
  • 控制算法包含了MPC、PP、Stanley、LQR、PID等; 包含m源文件以及mdl模型;
  • PID算法基础 首先我们需要去了解PID算法的数学原理,数学原理部分借鉴于 @确定有穷自动机 的博客——一文读懂PID控制算法(抛弃公式,从原理上真正理解PID控制) 总的来说,当得到系统的输出后,将输出经过...
  • 飞控中的PID算法理解

    千次阅读 多人点赞 2018-09-29 16:30:18
    前言:这篇博客是对一位博主的博客的转载,地址是:https://www.cnblogs.com/kinson/p/9651434.html 为了防止网址后面失效,下面是这篇博客的截图。 ...
  • 飞行控制PID算法——无人机飞控

    千次阅读 2021-02-03 15:18:33
    PID控制应该算是应用非常广泛的控制算法了。小到控制一个元件的温度,大到控制无人机的飞行姿态和飞行速度等等,都可以使用PID控制。这里我们从原理上来理解PID控制。 PID(proportion integration differentiation)...
  • PID控制算法   PID控制器是一个结构简单并且成熟稳定的控制器,在工业上应用广泛。包括比例(Proportion)、积分(Integral)、微分(Differential)三个控制元素,三者是对系统偏差不同处理方式。 比例控制 P  ...
  • 1.刚体: 在运动中和受力作用后,形状和大小不变,而且内部各点的相对位置不变的物体。绝对刚体实际上是不存在的,只是一种理想模型,因为任何物体在受力作用后,都或多或少地变形,如果变形的程度相对于物体本身...
  • 飞控算法——互补滤波器

    千次阅读 热门讨论 2015-09-30 21:35:46
    在了解滤波器的开始先看几个概念 传感器 两轴加速计: 1.测量加速度实际上是推动每个单元的块。 2.可以用来测量重力加速度,上图中x轴加速度为0g,y轴加速度为1g。 3.可以用来测量倾角。...
  • 飞控系统PID算法

    2014-05-23 10:31:08
    本程序是使用C写的一个飞控系统中通用的增量型PID算法
  • 浅谈飞控的软件设计

    千次阅读 多人点赞 2019-01-21 11:12:34
    开这个专栏的目的主要是深感自己对飞控软件、算法的知识点过于杂乱,很久没有进行系统的总结了,因此决定写几篇文章记录一些飞控开发过程的知识点。主要是针对一些软件、算法部分进行讨论,如内容有错误,欢迎指出。...
  • 开源飞控的现状

    千次阅读 2021-02-08 15:32:16
    无人机能被快速普及,很大程度上是得益于开源飞控的发展,因为困扰着无人机发展的关键设备是自动驾驶仪。那么,开源飞控是什么?又是如何发展过来的? 在纷繁复杂的无人机产品中,四旋翼飞行器以其结构简单、使用...
  • 姿态融合算法、微小型无人直升机飞控平台与姿态融合算法研究_徐玉

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,832
精华内容 732
关键字:

飞控算法