0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看技术视频
  • 写文章/发帖/加入社区
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

导航及无人机姿态检测融合算法很难?用sensor fusion来搞定!

意法半导体AMG 来源:未知 作者:电子大兵 2017-09-05 09:47 次阅读

1.什么是姿态融合算法

简单来说,姿态融合算法就是融合多种运动传感器数据(一般需要3轴加速度, 3陀螺仪或者3轴地磁感应传感器),通过数字滤波算法容错补偿,实现当前姿态检测

2.数字滤波算法的选择

根据运动传感器噪声模型,一般以下滤波算法可供融合算法选择:

a)互补滤波算法

b)扩展卡尔曼滤波算法

c)无迹卡尔曼滤波算法

d)粒子滤波算法

e)Mahony互补滤波算法

3.ST姿态检测融合算法

ST能提供完整的,高性能姿态检测融合算法--MotionFX/iNemoEngine。其能最大限度规避因加速度和地磁感应传感器数据异常,或者陀螺仪零飘带来的误差。多种数据输出选择,包括四元素,欧拉角,线性加速度以及重力矢量。

还有9轴和6轴两种选择:9轴的话需要3轴加速度,3轴陀螺仪和3轴地磁感应,能够提供当前三维空间的绝对方向信息可以用作导航及无人机姿态检测等应用;6轴需要3轴加速度和3轴陀螺仪数据,可用于游戏,AR/VR等应用。

应用案例——GD32F103+MPU9150四旋翼飞行器第一步:姿态融合算法

前言:

相比直升机来说,四旋翼乃至多旋翼飞行器的机械结构简单,操控灵活,飞行稳定,体积也能做的更小,当然也能更大,它将直升机复杂的机械结构设计难度转化到了电子电路和算法上面,因此四旋翼飞行器的设计更容易上手,更民众化。

四旋翼飞行器的软件核心包括两大部分:姿态融合算法和控制算法;硬件核心便是MCU和传感器。

先介绍下姿态融合算法,姿态融合说白了就是将3轴加速度、3轴角速度和3轴磁场强度融合成四元数,再将四元数转化为欧拉角,最后将欧拉角最为控制量输送到所有电机以达控制飞行器姿态的目的。欧拉角包括偏航角Yaw、俯仰角Pitch和滚动角Roll。我用的算法是Madgwick写的AHRSUpdate和IMUUpdate,简单有效,其中AHRSUpdate是融合了陀螺仪、加速度计和磁力计,而IMUUpdate只融合了陀螺仪和加速度计,就优缺点来说,IMUupdate算法只融合了加速度计和陀螺仪的数据,还需要使用互补滤波算法来融合磁力计以修正偏航角Yaw,不然飞行器会找不到北,但是这种互补滤波有个小问题,就是假如定义偏航角的范围是0-360度,那么当机头大概从北偏西1度转到北偏东364度时,机头会经过0度(360度)这个点,那么这时,yaw不会直接1-0-364这么变化,而是会被逆向积分从1-20-180-270-364这么转一圈,这是个不好的现象,实验了半天也没有解决,而AHRSupdate很好的解决了这个问题,不过由于AHRSupdate把磁力计的数据融合进了所有欧拉角,因此当传感器受到外围强磁场干扰时,就会造成全方位失控,导致坠机,而使用IMUupdate算法,顶多飞行器会转圈而已。

传感器我用的是invensense公司的MPU9150,MPU9150芯片集成了加速度计、陀螺仪和磁力计,并且内置硬件DMP用于姿态融合,不过不好用;MCU则用是Gigadevice公司的GD32F103系列,由于我也是刚接触四旋翼飞行器,第一个目的当然是能够平稳的飞起来,暂不考虑加入其他外围设备。后续可能会考虑使用GD32F107或者GD32F2xx系列,可扩展摄像头小玩一把航拍,当然更好的是GD32F4xx系列(期盼中),自带浮点运算单元,由于我软件太菜,算法中出现大量的浮点运算导致姿态更新频率和控制频率达不到很高。

做四轴飞行器也是为了好玩,目前我只完成了第一步:姿态融合。接下来才是更重要的,选择合适的机架、电调、电机、螺旋桨,写PID控制代码,系统整合以后还要调试各种参数,抗干扰,抗震动,最后还要加各种应用器件。在此鼓励一下自己,坚持就是胜利,慢慢磨洋工。

我现在软件实现的功能:算法用AHRSupdate、陀螺仪零偏校准、加速度计平滑滤波、磁力计平面校准,以后看情况可能会慢慢更新加速度计精确校准、磁力计椭球拟合校准、陀螺仪温度补偿等。

第一部分:硬件

1.传感器:MPU9150(INVENSENSE公司的,单芯片内集成了加速度计、陀螺仪和磁力计,并且内置DMP用于姿态融合,不过只融合了加速度计和陀螺仪,没有融合磁力计进去,具有自校准功能,价格比MPU6050贵很多,但是省PCB面积,省事,轴向重合度高。实际上就是把MPU6050和磁力计AK8975放在同一个芯片里,程序还是使用MPU6050的驱动,缺点是会偶尔丢失数据,自带的姿态融合算法的更新频率不高);

2.MCU:GD32F103CB(Gigadevice公司的,ARMcortex-M3内核,32位MCU,主频最高108Mhz,48Pin,与ST同型号的32位MCU 直接兼容,性价比更高,外接8M晶振,也可使用内部8M晶振,晶振远离传感器,避免干扰磁力计,不过当时考虑不周,这个芯片的timer太少了,以后会先采用GD32F103VCT6或者GD32F107VCT6);

3.电源芯片:TLV70233DBVR(TI的LDO,输入2-6V,输出3.3V,只需要外接2个X7R无极性陶瓷电容,价格太高)

4.串口:MAX3232(方便调试)

5.USB供电,输出电压5V

6.目前机架、电调、电池、电机和螺旋桨已买好,来张图,比较大众化:

上图:

图1:PCB的3D效果图,测试版,先追求调通得出姿态角,后续改版会做大的调整:

第二部分:软件

1.使用keil,uvision4.1.0,工具链:RealViewMDK-ARM Version4.12;

2.驱动:官方的MPU6050驱动inv_mpu.c和inv_mpu_dmp_motion_driver.c;

先看几个图,然后再说坐标轴的设定和算法部分。

上图:

图1:系统初始化,顺序从上到下依次是:初始化MPU、设置需要使用哪些传感器、设置陀螺仪测量范围(我设的是正负500度/s)、设置加速度计测量范围(我设的是正负4g)、配置fifo、设置采样率、装载DMP、设置陀螺仪轴向(比较重要)、使能DMP的一些玩意儿、设置DMP的FIFO、自校准陀螺仪和加速度计、开启DMP、开始姿态融合,见下图:

图2:由四元数求出的最终姿态角,其中Yaw为航向角,表示机头偏离正北方多少度,范

围-180到+180;Pitch为俯仰角,表示机头正方向与水平线的夹角,范围-90到+90;Roll为翻滚角,表示机翼与水平线的夹角,范围:-180到+180。下图为机身水平,且机头正北偏西37度左右的数据:

图3:下图为机翼水平,机头指向正北,且机头向下25度的数据

图4:下图为机头指向正北,保持水平,且机翼的右翼向下倾斜23度的数据

图5:看下欧拉角的奇异点,在奇异点处一个转动状态对应无穷多组自由度值,当物体转到这些奇异点附近,便没法求解。图中当Pitch为+90度时,机体的姿态便没法控制,Roll的轴向发生了变化。如下图:

第三部分:如何确定自己的轴向

首先,轴向的定义跟初始化四元数和最后结算的欧拉角有关,跟四元数更新算法无关,换句话说,不管你的轴向如何定义,姿态融合算法随便用,但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变,这个后面算法中有说。加速度计也好,陀螺仪也好,磁力计也好,他们的轴向都要满足右手定理,如下图:

再附上一段注释用于解释如何定义合理的轴向,以及如何正确旋转传感器的轴向,解释这么多其实就是说定义好的轴向要满足右手定理,如下图:

下图,旋转前是[x y z],旋转后就是[-y x z]:

下图是如何确定旋转角度的正方向,用右手握住坐标轴,拇指指向轴向的正方向,四个指头弯曲的方向就是旋转角度的正方向,在初始化四元数时,计算出的欧拉角的正方向也要满足这个条件:

我的程序使用的轴向如下图所示,未作任何改变:

第四部分:算法

第一步是校准,加速度计和陀螺仪我用的是MPU9150内部自校准,磁力计的校准采用如下方法:见附件-磁力计校准

第二步是初始化四元数,常见的轴向定义是绕x轴旋转是Roll,绕y轴旋转是Pitch,绕z轴旋转是Yaw,我的程序中也有这样的定义,不过被我注释掉了,这里举个另外一种轴向定义来对初始化四元数进行说明,方便比较,也是我目前正在用的轴定义。

下面我们来定义绕x轴旋转是Pitch,绕y轴旋转是Roll,绕z轴旋转是Yaw,轴向的正方向如上图一样,不变。

先对加速度计和磁力计的数据进行处理,得到init_xx来供我们使用如下图:

然后通过公式计算出初始化的Roll、Pitch、Yaw,注意加负号保证旋转角度的正方向,如下图:

其中Yaw的正方向未必对,可以自己去验证下,具体参考公式见附件-ST电子罗盘计算Yaw

然后由上面的欧拉角求出初始化四元数,这时要注意旋转顺序的不同,公式也不同,大部分旋转顺序是Z-Y-X,我的程序里也用的这个顺序,在这里我们按Z-X-Y的顺序来旋转,并得出求四元数的公式以做比较,其旋转矩阵:

q=qyaw*qpitch*qroll=

(cos(0.5*Yaw)+ksin(0.5*Yaw)) *(cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll))

得出初始化四元数计算公式如下图所示:

其中i,j,k之间相乘的顺序不能随意变,在前的先计算,在后的后计算相乘的公式如下图:

至此初始化四元数完成。

第三步就是使用AHRSUpdate算法了,用完以后再根据公式计算出欧拉角,此公式跟旋转顺序和旋转使用的轴向有关,我们的旋转顺序是Z-X-Y,且绕Z是Yaw,绕X是Pitch,绕Y是Roll,推到过程如下图:

首先得出3个方向余旋矩阵:

下图绕Z轴Yaw:

下图绕X轴pitch:

下图绕Y轴Roll:

然后按照我们的Z-X-Y顺序求得C=Croll * Cpitch * Cyaw,如下图:

将上图的方向余旋矩阵C与下图的四元数姿态矩阵做对比,即可求出欧拉角,注意上图的方向余旋矩阵C是随着我们对坐标轴的定义变化而变化的,而下图的四元数姿态矩阵是固定的:

最后一步就是求出欧拉角,公式如下图:

以下是静止时,刚上电时的数据:

以下是上电半个小时以后的数据:

可以看出正常的静止状态下,数据的波动范围是不超过1度的,也不会有漂移。具体效果还需要上机架飞起来以后再做调整。

最后总结一下:

我的代码中,0°

注意跑108Mhz时,需要修改延迟函数和串口函数,适当增加一下延迟函数的数值避免I2C通信失败,修改串口函数是为了避免108Mhz下串口乱码问题


声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 意法半导体
    +关注

    关注

    30

    文章

    2973

    浏览量

    107914
  • 运动传感器
    +关注

    关注

    9

    文章

    160

    浏览量

    30589
  • 地磁传感器
    +关注

    关注

    1

    文章

    40

    浏览量

    17982
  • 3轴陀螺仪
    +关注

    关注

    0

    文章

    3

    浏览量

    5525
  • 3轴加速度计

    关注

    0

    文章

    4

    浏览量

    7210

原文标题:ST sensor fusion-姿态检测融合算法简介

文章出处:【微信号:St_AMSChina,微信公众号:意法半导体AMG】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    第四集 知语云智能科技无人机反制技术与应用--无人机的组成与工作原理

    转弯等动作。 姿态控制:无人机姿态控制依赖于飞控系统和传感器。传感器实时感知无人机姿态、速度和位置信息,飞控系统根据这些信息计算出控制指
    发表于 03-12 11:28

    第三集 知语云智能科技无人机反制技术与应用--无人机的应用领域

    应对突发情况。 智能识别:通过先进的算法和数据库,能够识别不同品牌和型号的无人机,实现个性化反制。 四、无人机反制技术的应用前景 随着无人机技术的不断发展,
    发表于 03-12 11:13

    第二集 知语云智能科技无人机反制技术与应用--无人机的发展历程

    等问题层出不穷,给社会安全带来了严重威胁。为了解决这些问题,知语云智能科技投入大量研发力量,推出了一系列无人机反制技术。 知语云智能科技的无人机反制技术涵盖了检测、识别、追踪、干扰等多个环节。通过
    发表于 03-12 10:56

    第一集 知语科技无人机反制技术与应用--无人机的定义与分类

    技术的崛起 随着无人机技术的普及,无人机侵犯隐私、干扰航空安全等问题日益突出。为了应对这些挑战,无人机反制技术应运而生。无人机反制技术主要包括无线电干扰、
    发表于 03-12 10:42

    知语云智能科技无人机防御系统:应对新兴威胁的先锋力量

    作为知语云智能科技的创新产品,具备高效、智能、安全等特点。该系统通过先进的雷达探测技术,能够迅速发现目标无人机,并进行精确跟踪。同时,结合高速计算机处理技术和智能算法无人机防御系统能够准确判断
    发表于 02-26 16:35

    无人机全景监测:空域管理的新革命

    、交通管理、城市规划等多个领域发挥更大的作用。同时,随着5G、物联网等技术的融合应用,无人机全景监测技术也将迎来更多的创新和发展机遇。 结语 无人机全景监测技术作为提升空域管理效率的关键,正逐渐改变
    发表于 02-20 15:23

    Spring Boot和飞腾派融合构建的农业物联网系统-改进自适应加权融合算法

    数据融合之前需用箱线图法检测和剔除异常值。箱线图法中反映数据结构的参数有:第一四分位数 Q1、第三四分位数 Q3以及四分位间距IQR。 2自适应加权融合算法 为了确保参数估计的准确性和稳定性,本文
    发表于 01-06 12:18

    陀螺仪LSM6DSV16X与AI集成(3)----读取融合算法输出的四元数

    LSM6DSV16X 特性涉及到的是一种低功耗的传感器融合算法Sensor Fusion Low Power, SFLP). 低功耗传感器融合(SFLP)
    的头像 发表于 12-18 10:53 501次阅读
    陀螺仪LSM6DSV16X与AI集成(3)----读取<b class='flag-5'>融合算法</b>输出的四元数

    无人机导航技术分析

    导航的方法和手段有许多,最传统的是航图导航,最流行的是卫星导航,最具独立性的是惯性导航无人机上用到的是卫星
    发表于 11-29 11:41 1242次阅读
    <b class='flag-5'>无人机</b>的<b class='flag-5'>导航</b>技术分析

    知语云:低慢小无人机如何反制管制监测行为?方式方法又是什么?

    。雷达探测“低慢小”无人机则有以下突破口 (1)设计合适的雷达体制、信号频率等; (2)开发更高效的杂波抑制、自适应恒虚警检测、点迹聚集、脉冲压缩和单脉冲测角等关键算法。 具体可考虑以下内容 1
    发表于 11-20 17:07

    电磁干扰会对无人机产生怎样的影响 电磁干扰无人机的优缺点有哪些

    电磁干扰可能会扰乱无人机导航系统,如GPS或其他位置定位系统。无人机依赖这些导航系统来确定自身位置、飞行方向和高度等信息。干扰可能导致无人机
    发表于 09-26 15:51 3390次阅读

    无人机飞控算法的发展历程简述

    无人机飞控算法的基础是飞行动力学和控制理论。飞行动力学研究飞机在空气中的运动规律,包括姿态控制、稳定性和操纵性等方面。控制理论研究如何设计控制器来实现期望的飞行动作和轨迹。这些基础理论为无人机
    发表于 09-06 11:12 972次阅读
    <b class='flag-5'>无人机</b>飞控<b class='flag-5'>算法</b>的发展历程简述

    无人机最重要的三大功能是什么?无人机最重要的部分是什么

    飞控系统是无人机的核心,它包括飞行控制器和相关的传感器。飞行控制器负责接收和处理来自传感器的数据,并控制无人机的飞行。传感器可以包括陀螺仪、加速度计、罗盘、气压计等,用于感知无人机姿态
    发表于 08-15 15:11 1163次阅读

    无人机的陀螺仪是什么作用 无人机控制系统原理

    无人机的陀螺仪是一种用于测量和控制无人机姿态的重要传感器。它通过检测无人机的旋转运动来提供准确的方向和角度信息。
    发表于 07-21 15:02 3437次阅读

    无人机导航技术及其特点解析

    无人机导航是指借助计算机中心控制站实现一系列的飞行任务,为了保证无人机的可控性和可靠性,必须要确保无人机导航精度,这样才能够沿着预设航线飞行
    的头像 发表于 06-16 14:52 2137次阅读