搜索
查看
编辑修改
首页
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
微信小程序获取openId_微信小程序获取用户openid
2
(CVPR-2021)具有深度通用线性嵌入的跨视角步态识别_koopman算子
3
【计算机毕业选题】微信小程序毕业设计选题推荐_关于校园服务小程序的前端开发毕业论文选题有哪些
4
kafka如何保证数据不丢失?ack设置为 all和-1_ack all
5
第23篇 Android Studio第一个程序HelloWorld_android studio 4.2.2 helloworld
6
python实现学生管理系统(mysql+面向对象)_综合应用python程序设计的知识设计与实现一款简单的学生管理系统。需要连接mysql
7
Websocket服务端结合内网穿透发布公网实现远程访问发送信息_websocket透传
8
Flink 窗口&触发器
9
深入理解Android View绘制流程 源码详解
10
Python实现微信自动回复_python微信自动回复
当前位置:
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
【云原生】基于
windows
环境
搭建
Docker
_
windows
创建
docker
环境
...
基于
windows
环境
搭建
Docker
_
windows
创建
docker
环境
windows
创建
docker
环境
...
赞
踩
article
win10
上使用GIT慢的问题,或者
命令行
反应慢的问题_
window10
启动
命令行
程序
第一次
...
有的电脑
win10
命令行
或者 git bash
命令行
会出现反应缓慢,一般是因为 双显卡引起的。我的是 amd ...
赞
踩
article
五年磨一剑
,
华为
仓颉
编程语言
正式
发布
|附
入门教程
_
仓颉
什么时候
正式
发布
...
华为
在 2024 年 6 月 21 日的
华为
开发者大会上
,
华为
终端 BG 软件部总裁龚体
正式
官宣了
华为
自研
仓颉
编程语言
,
...
赞
踩
article
30
岁
以上
的
程序员
职业
出路
是什么?真
的
只能靠降低
生活
期望吗?_
30
岁
程序员
的
出路
...
32
岁
,年薪60万
的
程序员
在北京
的
生存状态是怎样
的
?达到这个标准
的
程序员
给出答案是——生存而不是
生活
,转型压力巨大,买不...
赞
踩
article
Python
之
网络
爬虫---
正则表达式
...
Python
之
网络
爬虫1.
网络
爬虫概念说明:
网络
爬虫就是从互联网中定向或者不定向的采集信息的一种程序。
网络
爬虫有很多类型...
赞
踩
article
用户
登录
控制
【
spring
boot
整合
spring
security
】_
spring
boot
用户
...
在前面几个功能的演示过程中,都需要预先使用Spring Security提供的默认
登录
页面和默认
登录
用户
user
登录
认证...
赞
踩
article
无脑歧视
项目
外
包
公司
,吹捧
自研
产品
公司
,都是不理性的。_
公司
又
外
包
又
自研
...
经常看到有人在网上说软件开发
外
包
公司
的各种不好,还说有
外
包
经历是简历的污点,贝格前端工场作为
项目
公司
(非驻场人力
外
包
),...
赞
踩
article
ULTRAINTERACT
数据
集
与
EURUS
模型
:推动开源大型语言
模型
在
推理
领域的新进展_st...
推理
任务,特别是那些需要综合运用数学知识、编程技能和逻辑分析的挑战,对LLMs提出了更高的要求。为了提升
模型
的
推理
能力,...
赞
踩
article
这才是
企业级
的
oss
-
spring
-
boot
-
starter
,真心牛x!...
程序员
的
成长之路互联网/程序员/技术/资料共享关注阅读本文大概需要 10分钟。来自:juejin.cn/post/721...
赞
踩
article
C语言例程:
插入
排序
_
要求
根据指定接口
,
使用
直接
插入
排序
算法
实现一组数
从小到大
排序
,
要求
输出每一趟的...
将一个整数数组按
从小到大
的顺序进行
排序
,主要学习基本的
插入
排序
和改进的冒泡
排序
的
算法
和应用。_
要求
根据指定接口
,
使用
直接
...
赞
踩
article
MySQL
周内训参照4、
触发器
-插入-修改-删除_
trigger
要求
:1、
用
户
表
添加
语句
添加
触发器
,...
MySQL
周内训参照4、
触发器
-插入-修改-删除_
trigger
要求
:1、
用
户
表
添加
语句
添加
触发器
,
要求
在
添加
用
户
信息
时
...
赞
踩
article
C++
_海量
数据处理
布隆
过滤器
(
位图
+
哈希
_
C++
实现_应用分析)...
C++
BitSet
位图
位图
的优缺点
位图
的优点:节省空间,速度快。
位图
的缺点:只可以处理整数。文章目录1.
布隆
过滤器
提出2...
赞
踩
article
RabbitMQ
:
队列
优先级
和
消息
优先级
的介绍和
使用
_
rabbitmq
优先级
...
1.声明当前内容主要用于本人学习和复习之用,当前内容包括如何创建
优先级
队列
和
使用
消息
优先级
,以及不设定
优先级
和设定
优先级
...
赞
踩
article
人工智能
与机器学习——
微笑
识别
和口罩
识别
_
提取
人脸
某一
特征
的潜在向量比如笑脸...
人工智能
与机器学习——
微笑
识别
和口罩
识别
一、
人脸
图像
特征
提取
方法简介HOG
特征
提取
步骤(1)输入图像并检测窗口(2)伽马...
赞
踩
article
【大
语言
模型
LLM
】-
如何
使用大
语言
模型
提高
工作
效率
?_大
语言
模型
在日常办公中
如何
帮助提升
文本处理
效...
接上一章,这一节我们开始学习大
语言
模型
的常见使用场景,对于我们普通人而言,大
语言
模型
的基础使用场景有哪些?
如何
使用大
语言
...
赞
踩
article
各种常用
的
JSON
接口
,
值得收藏
_
每个
接口
的
json
...
今天在OSC上
,
看到“能够返回
JSON
格式
的
服务
接口
”
的
一篇文章
,
觉得还不错
,
就转载过来
,
大家分享。这里为大家搜集了一些...
赞
踩
article
读书笔记
—
—
吴翰清
《白帽子讲
Web
安全
》...
目录第一篇 世界观
安全
一 我的
安全
世界观第二篇 客户端脚本
安全
一 浏览器
安全
二 跨站脚本攻击(XSS)三 跨站点请求伪造...
赞
踩
article
互联网
+比赛商业
计划书
模板 大一
,
要参加“
互联网
+”
大学生
创新创业
大赛
有什么合适
的
项目
(0基础)?_...
互联网
+比赛商业
计划书
模板 刚开始做竞赛
,
特别是大一对于这个社会
的
市场需求、自己
的
都比较少
,
如果纯自己这篇文章
,
结合我
的
...
赞
踩
article
python
通过
selenium
爬取
网页
信息,
python
获取
浏览器
请求内容,控制已经
打开
的
浏览器
_...
python
爬虫,
通过
selenium
爬取
网页
,操作
网页
,执行js,
获取
请求响应,控制
浏览器
行为。_
python
获取
已经...
赞
踩
article
C++
用
快速
排序
法对一组
数据
进行
从小到大
排列_
c++
一维
数组
排序
从小到大
...
快速
排序
法:通过一趟
排序
将要
排序
的
数据
分割成独立的两部分,其中一部分的所有
数据
都比另一部分的所有
数据
要小,然后再按此方法...
赞
踩
相关标签
win10搭建docker
windows搭建docker
win10安装docker
华为
python
开发语言
生活
python入门
Python兼职
网络爬虫
正则表达式
spring
spring boot
java
外包公司
人工智能
数据集
大模型
自然语言处理
深度学习
机器学习
推理能力提升
后端
c语言