Pixhawk代码分析-姿态解算篇D
姿态解算篇D
基础知识
研究多旋翼无人机前期主要需要了解其气动布局和复杂的动力学模型,然后就是姿态解算和控制器的设计。为了实现精确四旋翼飞行器的姿态估计,首先就是需要了解各传感器采集的数据和误差存在的原因,然后就是各种数学运算(都是基于一定的数学平台),比如DCM随时间的变化、四元数微分方程、四元数随时间的变化、重正交化等等。
在理解构建四旋翼动力学模型时,需要了解它是属于四输入六输出的欠驱动系统,欠驱动主要就是输入只值靠四个电机的拉力实现控制角速度、角度、上升速度、下降速度、上升距离和下降距离。动力学模型还在研究中。
实现四旋翼飞行器的自主飞行主要就是靠姿态解算和姿态控制。姿态控制是四旋翼飞行器实现各项工作的核心,位置控制建立在完善的姿态控制的基础上。前面已经介绍过,仅仅依靠陀螺仪积分获取角度值是不可靠的,由于陀螺仪由于其本身的固有的drift,积分带来的误差随时间的积累越来越大;而加速度计又容易受到噪声和震动的干扰。
因此在此基础上使用各种算法对两种数据进行融合,以获取较为理想的姿态数据。比较常见的融合算法有EKF(Extended Kalman Filter)、UKF(Unscented Kalman Filter)、梯度下降法、CKF(Cubature Kalman Filter)、PF(Particle Filter)和CF(Complementary Filter)。
其中mahony姿态解算算法主要就是基于CF设计的数据融合滤波器,该算法需要重点掌握其原理思想其分为AHRS(陀螺仪、加速度计、磁力计)和IMU(陀螺仪、加速度计)两种。
目前研究多集中在非线性控制领域,由于非线性控制对模型准确性有较强的依赖,在模型误差存在的条件下,PID控制更加实用,所以现有的基于mahony互补滤波器设计的算法中大多数都是基于PID反馈控制器的。
接下来就是使用Simulink进行控制器建模仿真,也在研究中。
代码算法分析
1、传感器的数值模型(mahony互补滤波器)
首先还是贴图吧,word编辑的公式还是搞不进来。
2、互补滤波器的设计(基于四元数)
这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值,定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。短时间内用陀螺仪比较准确,以它为主;长时间用加速度计比较准确,这时候加大它的比重,这就是互补了,不过加速度计要滤掉高频信号,陀螺仪要滤掉低频信号,互补滤波器就是根据传感器特性不同,通过不同的滤波器(高通或低通,互补的),然后再相加得到整个频带的信号。
例如,加速度计测加速度值,其动态响应较慢,在高频时信号不可用,所以可通过低通滤波器抑制高频干扰;陀螺仪响应快,积分后可测倾角,不过由于零点漂移等,在低频段信号不好,通过高通滤波器可抑制低频干扰。将两者结合,就将陀螺仪和加速度计的优点融合起来,得到在高频和低频都较好的信号,互补滤波需要选择切换的频率点,即高通和低通的频率。
在深入一点
1、对mahony互补滤波算法和PX4Firmware源代码中的姿态解算算法的一点见解
首先是最原始mahony滤波算法中的加速度计数据修正陀螺仪数据,源码如下。
1. // measured by Accelerate sensor
2. ax = ax / norm;
3. ay = ay / norm;
4. az = az / norm;
5.
6. // estimated direction of gravity (v)
7. vx = 2*(q1q3 - q0q2);
8. vy = 2*(q0q1 + q2q3);
9. vz = q0q0 - q1q1 - q2q2 + q3q3;
10.
11. // error is sum ofcross product between reference direction of fields and direction measured by Accelerate sensor
12. ex = ay*vz - az*vy ;
13. ey = az*vx - ax*vz ;
14. ez = ax*vy - ay*vx ;
15.
对上述代码的理解:[ax,ay,az] 表示由机体上的加速度计实际测量到的加速度数据,根据数据融合修正原理,加速度计在低频的情况下效果比较好,陀螺仪在高频情况下效果好;在低频下,忽略机体的运动加速度,即加速度计测量到的数据就是近似的纯重力加速度向量。[vx,vy,vz]表示由陀螺仪积分以后的姿态矩阵中获取的重力加速度向量(如何得到的不再赘述)。并且这两个向量都是在机体坐标系中,两个重力加速度向量做外积得到的向量就是所谓的误差向量(12、13、14行代码),且叉积的大小和陀螺仪的积分误差成正比,用该误差向量修正下一次的陀螺仪数据即可达到用加速度计修正陀螺仪数据误差的效果。
然后就是PX4Firmware源码中的加速度计数据修正陀螺仪数据,源码如下。
1. // Accelerometer correction
2. // Project 'k' unit vector of earth frame to body frame
3. // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); n系到
4. // b系
5. // Optimized version with dropped zeros
6. Vector<3> k(
7. 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
8. 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
9. (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
10. );
11. corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
12.
该算法中,Vector<3> k就是由陀螺仪积分以后的姿态矩阵中获取的重力加速度向量(等同于mahony算法中的[vx,vy,vz]);这部分都类似,主要的不同就是下面的加速度计测量数据的处理,首先通过加速度计获取总的加速度向量,然后同过GPS或者空速管获取运动加速度向量,然后通过这两个向量之差求取纯重力加速度,即:总的加速度 =运动加速度 + 重力加速度(此处考虑了低频情况下的运动加速度,修正效果更好)。_w_accel代表权重,即类似于Kp。
上述代码中的“%”运算符定义为叉积运算:
1. Vector<3> operator %(const Vector<3> &v) const {
2. return Vector<3>(
3. data[1] * v.data[2] - data[2] * v.data[1],
4. data[2] * v.data[0] - data[0] * v.data[2],
5. data[0] * v.data[1] - data[1] * v.data[0]
6. );
7. }
8. };
Pixhawk代码分析-姿态解算篇D相关推荐
- Pixhawk代码分析-姿态解算篇C
姿态解算篇C 前言 终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然 ...
- Pixhawk代码分析-姿态解算篇A
姿态解算篇A 基本知识 1.如何实现控制 一个无人机系统的算法主要有两类:姿态检测算法.姿态控制算法.姿态控制.被控对象.姿态检测三个部分构成一个闭环控制系统.被控对象的模型是由其物理系统决定,设计无 ...
- Pixhawk代码分析-姿态解算篇B
姿态解算篇B 前言 本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是William Premerlani and Paul Bizard的关于DCM的一篇经典论文< ...
- Pixhawk之姿态解算篇(5)_ECF/EKF/GD介绍
一.开篇 很久没更新blog了, 最近研究的东西比较杂乱,也整理了很多东西,没有来的及更新,最近发现很多小伙伴都开始写blog了,在不更新就要"被落后了".兄弟们,等等我啊~~~ ...
- Pixhawk之姿态解算篇(2)_mahony算法分析
一.开篇 还是没能进入到源码部分研究,对姿态解算过程太过于模糊,所以主要开始研究一下关于姿态解算的过程和实现,本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是Willia ...
- Pixhawk之姿态解算篇(4)_补充篇
一.开篇 大家期待已久的第四篇来了,但是本篇可能比较水啊~~~见谅~~~ 首先,上一周没有什么收获,虽然看了不少的论文,但是却没有什么质的飞越~~~~ 看的论文都是关于姿态解算的,用的算法大部分也都是 ...
- Pixhawk之姿态解算篇(3)_源码姿态解算算法分析
一.开篇 终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然后再做什么 ...
- 基于STM32的四旋翼无人机项目(二):MPU6050姿态解算(含上位机3D姿态显示教学)
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
- 关于姿态解算与融合的代码注释篇(三)
https://blog.csdn.net/sinat_23338865/article/details/53886114 加速度计和陀螺仪都能计算出姿态,但为何要对它们融合呢,是因为加速度计对振动之 ...
最新文章
- 看一个穷人怎样拥有多套房产的?-财富可以这样成长
- statistics DATA in SHANGHAI
- pytorch选出数据中的前k个最大(最小)值及其索引
- 数据分析工具篇pyspark应用详解——Pyspark实现PCA主成分
- 富人为什么赚钱这么容易
- N个结点不同结构的二叉树个数
- CMake is re-running because generate.stamp.list is missing.
- 如何取消html中页眉的下划线,Word文档页眉的横线怎么去掉?页脚的横线怎么去掉?...
- 虚无世界java路_我的世界攻略大全026-虚无世界3全新版介绍(上)
- k8s集成containerd,集成crictl工具,集成kata
- 增量式编码器和绝对式编码器
- 深度学习计算模型中门函数的作用
- Python绘制双对数曲线
- ant-design-vue中自定义a-tree的打开与折叠图标
- 大数据早报:Firebase推出机器学习功能 英伟达大幅扩大深度学习学院规模(11.2)
- CVPR 2018 论文解读集锦
- interlib android客户端开发,基于Android的移动图书馆设计与开发
- 【Qt 中出现 No such slot 错误 解决方法】
- 夏培肃对计算机科学发影响,夏培肃:我国计算机领域的先驱者
- 使用Scratch 3.0制作弹球游戏(一)——课题准备
热门文章
- Redis使用场景一,查询出的数据保存到Redis中,下次查询的时候直接从Redis中拿到数据。不用和数据库进行交互。...
- (转载)grep的使用
- NavigationController 已经洗干净了, 就等你来
- IE6 PNG 透明的方法
- arp攻击源代码(收集)
- PHP-代码审计-文件删除
- [数据库] Navicat for MySQL定时备份数据库及数据恢复
- 2018\National _C_C++_B\1.换零钞
- 征战蓝桥 —— 2014年第五届 —— C/C++A组第1题——猜年龄
- 《算法竞赛入门经典》习题4-2 正方形 (Squares,ACM,ICPC World Finals 1990,UVa201)——仅提供大体方法