搜索
查看
编辑修改
首页
UNITY
NODEJS
PYTHON
AI
GIT
PHP
GO
CEF3
JAVA
HTML
CSS
搜索
你好赵伟
这个屌丝很懒,什么也没留下!
关注作者
热门标签
jquery
HTML
CSS
PHP
ASP
PYTHON
GO
AI
C
C++
C#
PHOTOSHOP
UNITY
iOS
android
vue
xml
爬虫
SEO
LINUX
WINDOWS
JAVA
MFC
CEF3
CAD
NODEJS
GIT
Pyppeteer
article
热门文章
1
spring boot整合RabbitMQ —— 十分钟急速上手_docker run -it --rm --name rabbitmq -p 5672:5672 -
2
人脸识别系统---人脸对比_人脸识别 过程 输入人脸 对比图库 识别结果输出
3
DS-TransUNet
4
2024年第十六届“华中杯”(C题)大学生数学建模挑战赛| 曲率,多目标优化| 数学建模完整代码+建模过程全解全析_第十六届“华中杯”大学生数学建模挑战赛题目
5
Python跳动的爱心(双爱心版)_(13 * cos(angle) - 5 * cos(2 * angle) - 2 * cos(3
6
前端代码风格与提交规范_代码提交风格
7
软件测试 | 测试开发 | Android动态权限详解_android fragement动态申请权限
8
kimi协助完成论文写作数据分析,真的香!
9
Android View解析_安卓view详解
10
阿里Java后端面经【2021-8月份-记录牛客】_springboot 同一个消费组,不同消费者未消费
当前位置:
article
> 正文
飞行姿态解算(二)_mahonyahrs姿态解算偏航角没有变化
作者:你好赵伟 | 2024-06-27 17:41:19
赞
踩
mahonyahrs姿态解算偏航角没有变化
继之前研究了一些飞行姿态理论方面的问题后,又找到了之前很流行的一段外国大神写的代码,来分析分析。
先贴上代码:
#include "AHRS.h"
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f
#define Ki 0.005f
#define halfT 0.5f
//----------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
//----------------------------------------------------------------------------------------------------
// Function
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
代码不长,但是数学理论基础却是十分扎实的,必须要要有之前(一)的铺垫。
一段段分析代码:
#define Kp 2.0f
#define Ki 0.005f
#define halfT 0.5f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
前三行是一些常数的定义,包括Kp参数,Ki参数,采样时间。
后两行是一些变量的申明,q0,q1,q2,q3分别代表四元数的4个参数a,b,c,d,v=q0+q1i+q2j+q3k
exInt,eyInt,ezInt代表误差的积分项。这个后面会有说明。
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
来说说这些变量代表的意义,再注意各个参数是相对于哪个参考系的。这是算法理解的重点。
先说形参的
gx gy gz 表示陀螺仪读出的数据,分别为绕飞行器参考系 x轴,y轴,z轴正方向旋转的角速度。
ax ay az 表示加速度计读出的数据,分别为重力加速度在 飞行器参考系 x轴,y轴,z轴的分量。
mx my mz 表示磁场传感器读出的数据,分别为地磁场在 飞行器参考系 x轴,y轴,z轴的分量。
norm是做归一化的中间变量。
hx, hy, hz 表示将飞行器参考系上的地磁矢量转换到地理坐标系(参考坐标系)后的矢量。
bx,bz 与上面一样,(注意)区别是 bx在算法里为sqrt(hx*hx+hy*hy)这实际上是建立了一个误差函数,bx越接近hx,则整个估计姿态与电子罗盘测得的姿态越重合。
vx, vy, vz为将 标准单位重力 转换到飞行器参考系后 各个坐标轴上的分量。(重力加速度)
wx, wy, wz为将 bx与bz又重新 转换到飞行器参考系后 各个坐标轴上的分量。(地磁场)这个地方是比较难理解的,为什么转换过来又转换回去,后面会说。
ex, ey, ez 为向量a和向量m,与向量v和向量w的外积的在飞行器参考系的 向量。该向量描述了a和m与v和w的偏差程度。
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
这是些预先计算的数据,方便后面计算使用。
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
对加速度传感器和磁场传感器得到的数值进行归一化,方便计算。
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
利用四元数变换坐标系。具体为什么这样变换上一张图,笔记一里有详细过程。
上图为从飞行器坐标系转换到地理坐标系的余弦矩阵的四元数表达。
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
bx在当由磁场得到的飞行器参考系和由所有参数融合得到的飞行器参考系重合的时候理论上是等于hx的,此时hy应该等于0
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
反向使用上图余弦矩阵的四元数表达。及把a替换成-a,原理为四元数表达旋转时,a为旋转的角度,所以想旋转回去只需要把a变成-a。
(vx, vy, vz) 为一标准重力在飞行器参考系上的矢量。
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
同理,反向使用余弦矩阵的四元数表达。
这里可能大家已经发现,怎么wx wy wz是由传感器得到的。
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
e 为误差向量 他是 向量a和v的外积加上m和w的外积。
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
对误差进行积分;
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
用误差的积分和误差本身与Kp的乘积的和对角速度进行补偿。
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
利用龙格-库格法求出四元数的值,此值由补偿后的角速度求出。
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
对四元数进行归一化。
至此算法结束。剩余的事情就是把四元数转化为欧拉角,即可求出我们可以理解的飞行器的姿态yaw,roll,pitch
另外稍微分析下这个算法的实现原理。
a与v的外积 结果为 sinθ |a||v|n n为垂直于a,v组成平面的单位向量,方向遵守右手定理。由于都是归一化的,所以,这个结果的大小与θ有关,这个θ为a与v的夹角,因为a与v在halfT时间内的区别应该非常小,所以,此时θ=sinθ,这时候可以说这个外积的每个参数与θ成正比。
这个误差是怎么来的?就是因为飞行器在这短时间旋转了一定角度。
而从另一个角度,如果单纯从陀螺仪,同样可以得到飞行器旋转的一个角度。这个时候就有个问题?我们选择相信谁的?
我们谁也不相信,而是选择按一定比例相信了3个参数,e(误差),g(陀螺仪角速度)和eInt(误差的积分),这样我们就融合了这三个参数,最终得到我们的姿态。
PS:此方式在数学上在算法上没有任何问题,但是由于地磁传感器极易受到各种干扰(想想百度地图中的指南针精确度),而此算法又将地磁传感器所指示的方向过度的融入到了姿态当中,导致实际使用中数据会非常不稳定。我也是在此之后才发现。所以之后我使用了另一种融合算法,这个留到下一次再说。
声明:
本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:
https://www.wpsshop.cn/w/你好赵伟/article/detail/763243
推荐阅读
article
ARTS
Week
33...
她的新家将位于距离船只前往加尔迪·苏格杜布的港口两公里处,这是一项价值1200万美元的政府开发项目。巴拿马政府表示,加尔...
赞
踩
article
正
则
表达式
在网页处理中
的
应用
四
则
...
正
则
表达式
(Regular Expression)为字符串模式匹配提供了一种高效、方便
的
方法。几乎所有高级语言都提供了...
赞
踩
article
从
摄像头
中检测
脸
并
保存
图像
_
摄像头
人
脸
识别
保存
图片
...
改写自:http://docs.opencv.org/doc/tutorials/objdetect/cascade
_
c...
赞
踩
article
探秘
开源
隐语
:
架构
深度剖析与
隐私
计算技术
之旅_
隐语
框架
...
探秘
开源
隐语
:
架构
深度剖析与
隐私
计算技术
之旅_
隐语
框架
隐语
框架
...
赞
踩
article
人工智能
(
AI
)
技术
作
为
当今科技创新
的
前沿
领域
,
为
创业
者提供了广阔
的
机会和
挑战
_ai给
创业
带来
的
机遇...
本文探讨了
人工智能
技术
为
创业
者带来
的
机遇
,
如医疗、金融等
领域
的
应用
,
但也指出了数据资源需求、市场竞争和伦理法律问题。文章...
赞
踩
article
【
接口
】常见
接口
集合
(
返回
JSON
)...
转《
JSON
校验网站…》http://www.bejson.com/go.html?u=http://www.bejso...
赞
踩
article
BERT
源码
实现与解读(
Pytorch
)
_
bertmodel
pytorch
源码
...
使用nn.Transformer构建
BERT
模型,并使用样例样本使用MLM任务和NSP任务训练
BERT
_
bertmode...
赞
踩
article
【
测试
】23.
没有
文档
如何
定
测试
需求
_如果
没有
需求
文档
,
直接给
你
待测软件
,
你
将
如何
开展
测试
工作?...
相信做
测试
的大部分朋友都有同感,在很多时候,我们都要面临
没有
文档
或
文档
混乱,残缺的系统进行
测试
,而这时往往距高系统提交的...
赞
踩
article
一个
可一键
生成
短
视频
的AI大模型,亲测可用_
java
使用
ai
自动
生成
视频
...
MoneyPrinterTurbo ——
一个
利用大模型,一键
生成
短
视频
的开源项目。只需输入
视频
主题或关键词,就可以全自...
赞
踩
article
单片机
的
肺活量
检测仪
毕业设计
_
csdn
单片机
肺活量
...
肺活量
测量仪在医疗部门以及学校等地方应用广泛,实现智能化、高精度、便携化是
肺活量
测量仪现在的发展方向。随着
单片机
、微电子...
赞
踩
article
NineData
和
华为
云在一起!提供一站式
智能
数据库
DevOps
平台
...
NineData
作为新一代的云原生
智能
数据管理
平台
,提供了数据复制、
数据库
DevOps
、数据备份、数据对比等多种功能...
赞
踩
article
【
GitHub
项目推荐--
一个
独立的
C++
机器人
库】【转载】_
c++
机器人
库...
RL涵盖了数学、运动学和动力学、硬件抽象、运动规划、碰撞检测和可视化等方面,为
机器人
应用的开发提供了全面的支持。3. 硬...
赞
踩
article
C语言
中
的
结构
体——
struct
_
struct
需要
什么
头文件
...
结构
体是
C语言
中重要
的
概念,可描述复杂类型。文章介绍了
结构
体
的
定义、初始化、赋值、以及打印成员
的
方法。
struct
需要
什...
赞
踩
article
opencv
-
python
车牌
识别_
opencv
车牌
识别
python
...
opencv
-
python
车牌
识别本文尚有很多不足的地方,例如
车牌
字符的定位,
车牌
种类的不同,复杂环境下
车牌
识别的问题等...
赞
踩
article
虚拟化
服务器
固态
硬盘
,如何在
vSphere
环境中充分利用
SSD
...
如果部署和管理得当,在VMware环境当中使用
固态
硬盘
可以提升虚拟机的性能表现。
SSD
在设计过程中模拟了传统的磁盘驱动...
赞
踩
article
机器
学习
的
概念、步骤、分类
和
实践_请你谈谈大
数据
机器
学习
的
基本思路
和
编程流程...
机器
学习
是人工智能(AI)
的
一个分支,它使计算机能够从
数据
中
学习
并做出决策或预测
的
技术,而无需对每个问题进行明确
的
程序编...
赞
踩
article
反
无人机
技术
详解...
高能激光作用范围相对较远。声学对抗措施利用
无人机
组件,如惯性传感器对特定频率的敏感性,由声信号产生装置在破坏性频率上发射...
赞
踩
article
RabbitMQ
的幂等
性
、
优先级
队列
和惰
性
队列
_
rabbitmq
幂等
性
...
本文详细介绍了幂等
性
在处理消息重复消费中的解决方案,包括唯一ID和指纹码机制,以及Redis的原子
性
实现。此外,还讲解了...
赞
踩
article
ixigua
解析
_最新全网免费
抖音
去水印
解析
api
接口
,支持免费对接公众号...
昨天
抖音
改视频改规则了,然后结果,基本上网上99%的
抖音
解析
都凉了,昨天奋斗了一下午,终于从它的app内发现了一点秘密,...
赞
踩
article
Java
【
直接
插入排序
】
算法
,
大白话式详细图文解析(附代码)_
java
直接
插入排序
...
学数据结构与
算法
怎么少得了排序? 七大排序
算法
之[
直接
插入排序
]详细图解
,
结尾附[总体对比分析]_
java
直接
插入排序
...
赞
踩
相关标签
javascript
正则表达式
vbscript
html
asp
border
table
开源
架构
人工智能
科技
json
bert
pytorch
深度学习
定需求
音视频
python
github
语言模型
单片机
stm32
华为云
NineData