找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 7680|回复: 4
收起左侧

四轴飞行器九轴姿态解算原理

[复制链接]
ID:231800 发表于 2018-9-2 15:35 | 显示全部楼层 |阅读模式
一、姿态轴系介绍
1、欧拉角姿态表示
欧拉(Euler) 角是描述姿态最直观的方法,尤其是在描述载体坐标系相对于对应的当地导航坐标系的运动时。(应用在惯性导航中)
姿态被分解为三个连续的转动过程,每次旋转所围绕的轴与前后旋转所围绕的轴正交。图1 展示了这种旋转过程,通过两个过渡坐标系ψ和θ ,将与坐标系β 对齐的坐标系,旋转到与坐标系α 对齐。
图1 欧拉角的转动
第一次的转动角度ψ是偏航角,绕β 坐标系和第一个过渡坐标系共同的z轴旋转,x 轴和y 轴转动, z 轴保持不变;下一个旋转角为俯仰角θ,绕第一个过渡坐标系和第二个过渡坐标系共同的y 轴旋转, x轴和z 轴转动;最后一个旋转角为滚动角φ御,绕第二个过渡坐标系和α 坐标系共同的z 轴旋转, y 轴和z 轴转动。
通过欧拉角描述从参考坐标系到目标坐标系投影轴的旋转,可方便地表示目标坐标系相对于参考坐标系的方位。因此,滚动、俯仰和偏航欧拉角φ和θ和ψ描述了目标坐标系α 相对于参考坐标系β 的方位。在特定的情况下,用欧拉角描述载体坐标系相对当地导航坐标系的姿态时,滚动角φ被称作侧倾角(bank) ,俯仰角比被称作仰角( elevation) ,偏航角ψ呻被称作航向角(heading) 或方位角( azimuth) 。
2、坐标转换矩阵
坐标转换矩阵(coordinate transformationmatrix) 是一个3 x3 的矩阵,用符号C 表示。通过左乘适当的坐标变换矩阵,一步就可以完成矢量在两个投影坐标系之间的转换。即,对任意矢量x
欧拉角通过下式就可以转化为坐标转换矩阵:
反之可有矩阵求得欧拉角
2、四元数姿态表示
四元数( quatemion),即由四个元素组成的超复数:
q = (q0 ,q1,q2 ,q3)
通过下式可完成四元数与坐标转换矩阵之间的变换:
四元数和欧拉角之间的变换为
二、姿态解算步骤及原理
1、读取加速度计、陀螺仪、磁力计的数据;
2、重力加速度、磁力计数据归一化;
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;
3、重力分量(vx,vy,vz)提取,即将n系中z轴向量(0,0,1)通过坐标转换矩阵变换到b系中,变换后(vx,vy,vz)与(ax,ay,az)同处于b系中;
vx= 2*(q1q3 - q0q2);
vy= 2*(q0q1 + q2q3);
vz= q0q0 - q1q1 - q2q2 + q3q3 ;
4、求重力分量的姿态误差,向量(vx,vy,vz)与(ax,ay,az)误差通过向量叉乘计算获取;
ex= (ay*vz - az*vy)* Accel_magnitude;
ey= (az*vx - ax*vz)* Accel_magnitude;
ez= (ax*vy - ay*vx)* Accel_magnitude;
Accel_magnitude为加速度数据的可靠度
5、在n系中,地磁方向为恒定量,计做(bx,by,bz),其中(bx,by)合成矢量指向地磁场N极,假定bx对准地磁场N极,那么by= 0(头的定义)。
假如可以求出(bx,0,bz),则可按照与加速度计相同矫正法矫正。
地磁计在b系中的输出为(mx,my,mz),经过坐标转换矩阵(转置)变换到n系中,则有(hx,hy,hz),则有向量(hx,hy)与向量bx在XOY平面上合成矢量相同,n系中z轴向hz与bz相同,则有
bx2  = hx2 + hy2
bz = hz
即利用坐标转换矩阵求出了(bx,0,bz)。
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,0,bz)通过坐标转换矩阵变换到b系中得(wx,wy,wz),此时有(wx,wy,wz)与 (mx,my,mz)同时处于b系。
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);
6、求重力分量的姿态误差、磁力计的姿态误差,向量(wx,wy,wz)与(mx,my,mz)误差通过向量叉乘计算获取;
ex= ex + (my*wz - mz*wy);
ey= ey + (mz*wx - mx*wz);
ez= ez + (mx*wy - my*wx);
7、对重力分量与磁力计的姿态误差进行积分
exInt= exInt + ex * Ki;
eyInt= eyInt + ey * Ki;
ezInt= ezInt + ez * Ki;
8、互补滤波、将误差补偿到角速度上,修正角速度的积分漂移
gx= gx + Kp*ex + exInt;
gy= gy + Kp*ey + eyInt;
gz= gz + Kp*ez + ezInt;
9、利用一阶龙格库塔法(毕卡法)解四元数微分方程,更新四元数
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;
10、四元数归一化
norm= sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0= q0 / norm;
q1= q1 / norm;
q2= q2 / norm;
q3= q3 / norm;
11、四元素转换为欧拉角
Roll = -atan2(2 * q2 * q3 + 2 * q0 *q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.29578;
Pitch = -asin(-2 * q1 * q3 + 2 * q0*q2)*57.29578;
Mag_Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 *q3* q3 + 1)* 57.29578;
回复

使用道具 举报

ID:231800 发表于 2018-9-2 15:39 | 显示全部楼层
自己顶自己
回复

使用道具 举报

ID:1 发表于 2018-9-2 16:21 | 显示全部楼层
楼主能补下图片吗?
回复

使用道具 举报

ID:310519 发表于 2018-9-2 19:47 | 显示全部楼层
数学渣渣,看得头痛。

用MPU9250, DMP。

AK8963号称16位AD,不知道怎么得来的?
回复

使用道具 举报

ID:546770 发表于 2019-7-4 07:11 | 显示全部楼层
感谢!!
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

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