四元数姿态解算算法基础
文章目录
- 姿态的表示方法
- 四元数表示姿态的物理意义
- 使用四元数进行载体姿态更新方程
- 四元数微分方程
- 四元数初始值确定
姿态的表示方法
载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数。
欧拉角的物理意义比较直观,即航向角 ψ \psi ψ,俯仰角 θ \theta θ,横滚角 γ \gamma γ,分别是导航系到载体系的三个旋转角度。
姿态矩阵可以由欧拉角直接计算得到,即三个角度对应的转换矩阵依相乘。
注意的是导航系到载体系是按照航向角,俯仰角,横滚角的顺序变换的所以导航系到载体系变换时,三个矩阵相乘的顺序应该是 C ( γ ) ∗ ( θ ) ∗ C ( ψ ) C(\gamma)*(\theta)*C(\psi) C(γ)∗(θ)∗C(ψ).
四元数表示方法较为抽象,直接从四元数“很难”看出载体姿态(这个姿态是对人的想象来说的姿态,因为四元数和欧拉角本无本质区别,只是一种表示方法),那么他的物理意义是什么呢?
四元数表示姿态的物理意义
四元数表示姿态时有多个写法,其中一种表示方法如下:
q = c o s ( θ 2 ) + u n s i n ( θ 2 ) q=cos(\frac{\theta}{2})+u^nsin(\frac{\theta}{2}) q=cos(2θ)+unsin(2θ)
欧拉角表示姿态变换时,表征的是分别绕着 ( i , j , k ) (i,j,k) (i,j,k)三个坐标轴的三次旋转。根据欧拉定理,这三次旋转可以等效成绕着某轴一次旋转而成。这个某轴就是公式中的 u n = ( u 1 , u 2 , u 3 ) u^n=(u_1,u_2,u_3) un=(u1,u2,u3),旋转角度就是公式中的 θ \theta θ。
四元数通常写为 q = q 0 + q 1 + q 2 + q 3 q=q_0+q_1+q_2+q_3 q=q0+q1+q2+q3
那么用于姿态表示时,
q 0 = c o s ( θ 2 ) q_0=cos(\frac{\theta}{2}) q0=cos(2θ)
q i = u i ∗ s i n ( θ 2 ) , i = 1 , 2 , 3 q_i=u_i*sin(\frac{\theta}{2}),i=1,2,3 qi=ui∗sin(2θ),i=1,2,3
使用四元数进行载体姿态更新方程
q ( t + δ t ) = q ( t ) + d q d t × δ t q(t+\delta t) = q(t)+\frac{dq}{dt} \times \delta t q(t+δt)=q(t)+dtdq×δt
四元数的更新可通过上式完成。因此只要知道初始四元数(载体的初始姿态)和四元数的导数就可以完成任意时刻四元数的解算。
例如px4中姿态更新的代码如下
// Apply correction to state_q += _q.derivative(corr) * dt;
四元数微分方程
d q d t \frac{dq}{dt} dtdq 需要求解四元数微分方程,基本推导过程如下:
d q d t = − θ ˙ 2 s i n ( θ 2 ) + u n θ ˙ 2 c o s ( θ 2 ) + s i n ( θ 2 ) d u n d t \frac{dq}{dt}=-\frac{\dot{\theta}}{2}sin(\frac{\theta}{2})+u^n \frac{\dot{\theta}}{2}cos(\frac{\theta}{2})+sin(\frac{\theta}{2}) \frac{du^n}{dt} dtdq=−2θ˙sin(2θ)+un2θ˙cos(2θ)+sin(2θ)dtdun
其中, d u n d t = 0 \frac{du^n}{dt}=0 dtdun=0,因此:
d q d t = − θ ˙ 2 s i n ( θ 2 ) + u n θ ˙ 2 c o s ( θ 2 ) \frac{dq}{dt}=-\frac{\dot{\theta}}{2}sin(\frac{\theta}{2})+u^n \frac{\dot{\theta}}{2}cos(\frac{\theta}{2}) dtdq=−2θ˙sin(2θ)+un2θ˙cos(2θ)
= u n × u n θ ˙ 2 s i n ( θ 2 ) + u n θ ˙ 2 c o s ( θ 2 ) =u^n \times u^n \frac{\dot{\theta}}{2}sin(\frac{\theta}{2})+u^n \frac{\dot{\theta}}{2}cos(\frac{\theta}{2}) =un×un2θ˙sin(2θ)+un2θ˙cos(2θ)
= u n θ ˙ 2 × ( u n s i n ( θ 2 ) + c o s ( θ 2 ) ) =u^n \frac{\dot{\theta}}{2} \times (u^n sin(\frac{\theta}{2})+cos(\frac{\theta}{2}) ) =un2θ˙×(unsin(2θ)+cos(2θ))
= u n θ 2 × q = ω n b n × q 2 =u^n \frac{\theta}{2} \times q =\frac{\omega_{nb}^n \times q}{2} =un2θ×q=2ωnbn×q
结果中的角速度为导航系到机体系在导航系下的旋转角速度矢量,但是我们通过陀螺仪测得的是载体系下的角速度。因此还需要一部变换:
ω n b n = q × ω n b b × q ∗ \omega_{nb}^n=q \times \omega_{nb}^b \times q^* ωnbn=q×ωnbb×q∗
由此可得到,
d q d t = q × ω n b b 2 \frac{dq}{dt} =q \times \frac{\omega_{nb}^b}{2} dtdq=q×2ωnbb
我们得到了四元数导数和载体系角速度的数学关系,展开上式得 d q d t \frac{dq}{dt} dtdq:
[ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y ω x 0 ] × [ q 0 q 1 q 2 q 3 ] \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y\\ \omega_y & -\omega_z & 0 & \omega_x\\ \omega_z & \omega_y & \omega_x & 0 \end{bmatrix} \times \begin{bmatrix} q_0 \\ q_1 \\q_2\\q_3 \end{bmatrix} ⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0ωx−ωz−ωyωx0⎦⎥⎥⎤×⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
也可以写成,
[ q 0 − q 1 − q 2 − q 3 q 1 0 q 3 − q 2 q 2 − q 3 0 q 1 q 3 q 2 q 1 q 0 ] × [ q 0 q 1 q 2 q 3 ] \begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & 0 & q_3 & -q_2 \\ q_2 & -q_3 & 0 & q_1 \\ q_3 & q_2 & q_1 & q_0 \end{bmatrix} \times \begin{bmatrix} q_0 \\ q_1 \\q_2\\q_3 \end{bmatrix} ⎣⎢⎢⎡q0q1q2q3−q10−q3q2−q2q30q1−q3−q2q1q0⎦⎥⎥⎤×⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
四元数初始值确定
px4 中的初始四元数计算。
bool AttitudeEstimatorQ::init()
四元数初始值的确实可以看做是一个初始对准的过程。
而上述模型仅仅是理想状态下的数学模型,实际角速度中必然包含零偏、噪声等误差,更新过程中又不可避免的有飘漂移发生。所以仅用上式是无法给出可以使用的姿态的。
实际使用中常常用kalman滤波或者补偿滤波的方法来“校正”这些误差。因此初始值可以不必那么精确。给出一个概略初始值即可。
例如可以通过加速度z轴方向和磁罗盘的输出向量,来完成对准。两者从物理概念上实际上分别完成了调平和指北的任务。
四元数姿态解算算法基础相关推荐
- 详解几种飞控的姿态解算算法
姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...
- bvp解算器是什么_几种飞控的姿态解算算法
姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...
- 十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法<上>
这是"四轮车驱动控制"系列,分多个小节来介绍: 1. 八.四轮车驱动开发之一:正/逆向运动学分析 2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机 3. 十.四轮车驱动开发之 ...
- 一阶高通滤波+二阶Mahony滤波的四元数姿态解算
此次实验我使用icm20602进行 icm20602输出有以下特点: 3轴陀螺仪可选量程有±250dps,±500dps,±1000dps,±2000dps.(dps:degrees per seco ...
- 十三. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法<中>
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上) 十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中) 十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算 ...
- 十四. 四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法<下>
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(上) 十三.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法(中) 十四.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算 ...
- 四元数姿态解算及多传感器融合详细解析
代码路径ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp ...
- IMU:姿态解算算法集合
文章目录 一.IMU原理 二.源码 一.IMU原理 二.源码 源文件: #include "IMU.h" #include "math.h"#define Kp ...
- 四元数姿态解算c语言例程_(21)用四元数插值来对齐IMU和图像帧
最近一直在外实习,好久没更新博文了,实在罪过哈. 面临秋招,亚历山大,宝宝想哭,但要坚强. 前不久看到一句话,很有感触,送给一起秋招的小伙伴拉: 不要等到准备充足了才去开始,放出去溜溜吧拉1 说正事哈 ...
最新文章
- 经典题---数组改“貌”
- Flash中如何使用滤镜
- kafka重复消费问题
- Acwing 307. 连通图
- 在15分钟内使用Spring Boot和Spring Security构建一个Web应用程序
- 外星人跑深度学习_上海港汇外星人店,51M2020开光追和DLSS2.0畅玩《赛博朋克2077》...
- 思博伦Landslide CORE帮助UQ实现自动化的现网测试
- 开启协程_软件特攻队|GCC10发布,GCC 11将支持C++20协程功能?
- JS任务栏滚动效果问题
- 经纬度与度分秒相互转换
- Latex 箭头、下标、符号上下写文字、正方形和三角形
- wine android模拟器,Mac 下运行window 软件,(wine 模拟器)
- Solaris设置用户密码问题
- 第三周《java语言程序设计——面向对象入门》学习总结
- java模拟post发送文件
- 女程序员再漂亮也是单身?解析程序员圈的奇怪现象
- Linux 系统学习
- C语言小游戏之井字棋
- java 奇数中文乱码_java web 乱码 整理
- 20跨考计算机-拙笔