查看: 1353|回复: 0

【干货】详解几种飞控的姿态解算算法

[复制链接]

344

主题

383

帖子

1071

积分

金牌飞友

Rank: 6Rank: 6

积分
1071
飞币
686
注册时间
2017-7-6
发表于 2023-4-28 10:39:26 | 显示全部楼层 |阅读模式
文 / across

【作者简介】

不懂算法、不懂编程的飞控算法工程师
略懂飞行控制
略懂开源px4飞控

姿态解算是飞控的一个基础、重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障。有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多。主要是先掌握坐标系的概念、姿态角的几种描述方法(欧拉角、四元数、旋转矩阵)等。

姿态解算的难点主要在于,消费级飞控上一般所选用的惯性传感器,都是mems器件,精度相对较差,同时陀螺仪、加速度计、地磁计单个传感器无法得到满意的姿态角信息,所以需要一些融合算法,进行姿态估计。

首先,如何测量姿态解算的精度?

很多人都会有这个困扰,就是没办法确定所得到的角度的精确程度。一般用外部参考的方法测量,比如飞机上同时挂载自己的飞控以及高精度的IMU设备(比如xsens、sbg等),飞行完成后,对比自己飞控所解算的角度和外部设备的误差;又或者,在室内装vicon设备来给出外部参考。

笔者手上没有精确的外部设备,所以本文不能得出结论哪种算法的好坏,只是提供大概思路。

采用开源PX4飞控,进行姿态模式飞行,取出飞行数据,注意,验证算法的时候,最好还是用实际飞行的数据,否则加速度噪声对算法的影响无法验证,同时,这里的角度参考仅是px4飞控所解算得到的角度,不能算高精度参考,数据曲线:

【干货】详解几种飞控的姿态解算算法w2.jpg

【干货】详解几种飞控的姿态解算算法w3.jpg

1.看一下单独的陀螺仪积分角度与加速度计算出的角度各有什么问题?

陀螺仪角度:就是对角速度数据进行积分;

加速度角度:

【干货】详解几种飞控的姿态解算算法w4.jpg

在matlab中编写m代码进行验证对比。

【干货】详解几种飞控的姿态解算算法w5.jpg

如上图所示,蓝色的陀螺积分角度随着时间会有漂移,而加速度计得到的角度则噪声很大,都无法使用。

2.互补滤波

由于单一传感器获得角度都有缺点,一个动态精度好,一个静态精度好,很自然联想到互补滤波,即通过调整一个加权系数,如果陀螺仪所占的权重大些,则解算的角度则跟陀螺角度近似,加速度类似。

filtered(i) = (filtered(i-1) +IMU_GyroX(i)*IMUPer)*a + (1-a)*angle_acc(i);

a为0.95的曲线如下:

【干货】详解几种飞控的姿态解算算法w6.jpg

a为0.3,曲线如下:

【干货】详解几种飞控的姿态解算算法w7.jpg

综上所看,固定增益a,无论如何调整,性能总是不尽理想,陀螺仪的权重大了,动态性能还可以,大致能跟上角度,但是不能静态保持,当没有角速度时,角度也收敛了;加速度权重大了,噪音大,另外动态性能差,100s左右的来回打杆,所得的角度基本没有变化。

3.自适应互补滤波

如果权重参数可变呢?也就是如果角度变化大时,更相信陀螺,否则则更相信加速度的角度。

【干货】详解几种飞控的姿态解算算法w8.jpg

如图所示,虽然动态精度看上去好了,但是当静态的时候,还是不能达到满意效果。另外,图中可以看出,加速度的角度所保证的静态精度是有延迟的。

4.mahony&PX4SO3算法

PX4飞控早期的姿态解算有一个SO3版本,其算法其实就是网上常见的mahony算法。算法原理:根据加速度计和地磁计的数据,转换到地理坐标系后,与对应参考的重力向量和地磁向量进行求误差,这个误差用来校正陀螺仪的输出,然后用陀螺仪数据进行四元数更新,再转换到欧拉角。

具体步骤如下:

【干货】详解几种飞控的姿态解算算法w9.jpg

【干货】详解几种飞控的姿态解算算法w10.jpg

【干货】详解几种飞控的姿态解算算法w11.jpg

【干货】详解几种飞控的姿态解算算法w12.jpg

【干货】详解几种飞控的姿态解算算法w13.jpg

该算法能得到大概的角度。至于哪个更准确,这里无法判断。不过俯仰角,在40s左右的一段,这时候飞机姿态是回平的,但是解算有误。

5.卡尔曼滤波

卡尔曼滤波是非常常用的状态估计算法,相关的理论介绍有很多,这里不赘述了。有一点要注意,就是你的状态方程和量测方程的模型不一样,效果肯定也是不一样的,卡尔曼滤波只是理论,具体怎么用还得看具体情况,效果也是看所建的模型。

模型一:

状态量分别是俯仰角、滚转角以及对应的角速度偏移。

【干货】详解几种飞控的姿态解算算法w14.jpg

模型如下:

【干货】详解几种飞控的姿态解算算法w15.jpg

【干货】详解几种飞控的姿态解算算法w16.jpg

模型二:

参考书目

Nonami K, Kendoul F, Suzuki S, et al.Autonomous Flying Robots: Unmanned Aerial Vehicles and Micro AerialVehicles[M]. Springer Publishing Company, Incorporated, 2010.

第10章

【干货】详解几种飞控的姿态解算算法w17.jpg

结果如图:

【干货】详解几种飞控的姿态解算算法w18.jpg

两者曲线差不多,认为可以使用。

模型三:

PX4飞控早期的attitude_estimator_ekf版本。

【干货】详解几种飞控的姿态解算算法w19.jpg

【干货】详解几种飞控的姿态解算算法w20.jpg

【干货】详解几种飞控的姿态解算算法w21.jpg

算法的具体步骤,可以参考:

【干货】加速度校准分析(Pixhawk加速度校准算法简析)

【干货】详解几种飞控的姿态解算算法w22.jpg

几个小点:

1、怎么用卡尔曼进行融合?模型哪里来?

建模一般则是根据物理规律进行定的,运用现成的数学理论,如果不是很了解,可以参考一些论文。

2、卡尔曼如何调参数?

简单来讲,就是重点调R矩阵参数,一般取数据,用matlab看曲线。

3、数据的坐标系?

很多人用同样的代码,输入数据后,运算的结果不对。一般主要原因就是正负号不对应,要对好数据的坐标系,就是正负号,不知道的话,本办法就是试凑。

4、数据质量对解算的影响?

最后那个数据曲线就是手晃的数据,不是直接飞行的,加速度噪音小。数据质量一般关心的是加速度的数据,起飞后,加速度噪音比较大,可能你在地面测试,觉得姿态角算的挺准的,但是一起飞就不行了,这时候需要回过头来处理加速度数据,怎么处理?涉及到滤波方面的知识,后续会简单讲讲。

5、卡尔曼滤波好在哪里?

主要是因为一般的互补滤波,权重参数是固定的,而卡尔曼本质也是调整权重参数,但是是时变的,不过计算量相对较大。

6、动加速度的影响?

动加速度对姿态解算影响较大,因为你的陀螺是靠重力加速度向量去校正的,一旦运动的加速度大了,则会影响参考,自然也算不准了。普遍的方法是会融合GPS的速度算加速度。

这篇文章只是简单描述了一些常用的姿态解算算法,并不直接点明好坏,没有参考数据,不太好分析。有的一些没有给出推导过程,读者可以自己推导。一些理论也请自行查阅相关文献。后续也会讲解下卡尔曼滤波等内容。




【干货】详解几种飞控的姿态解算算法w23.jpg

每周更新

把握产业链脉络

追踪高价值的产品

坚持深度有价值内容
定位无人机频道
微信号:auscor

93.17%做灰机不吹牛逼的都可能关注了无人机频道!

最大的梦想是带着关注无人机频道的人一起发大财!

历史爆文

【干货】详解几种飞控的姿态解算算法w24.jpg

【视频】俄罗斯推出可骑无人机 载重120公斤最高能飞10米

【干货】陀螺仪的数据处理

【揭秘】在 DJI 大疆魔法部工作是怎样一番体验?

究竟是什么让植保无人机不赚钱?几乎都是一锤子买卖

这批无人机创业者真他Ma不行!
您需要登录后才可以回帖 登录 | 加入联盟

本版积分规则

快速回复 返回顶部 返回列表