小四轴姿态融合_asin(-2 q1 q3 + 2 q0 q2) 57

2401_85015293

于 2024-05-16 03:54:10 发布

阅读量224

点赞数 4

分类专栏： 2024年程序员学习文章标签：物联网嵌入式硬件面试

本文链接：https://blog.csdn.net/2401_85015293/article/details/138935280

版权

2024年程序员学习专栏收录该内容

13 篇文章 0 订阅

订阅专栏

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上物联网嵌入式知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频，并且后续会持续更新

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

如果你需要这些资料，可以戳这里获取

		```
		//normalise the measurements
		norm = invSqrt(ax*ax + ay*ay + az*az);      
		ax = ax * norm;
		ay = ay * norm;
		az = az * norm;
		norm = invSqrt(mx*mx + my*my + mz*mz);         
		mx = mx * norm;
		my = my * norm;
		mz = mz * norm;
		//从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值），下面这个是从飞行器坐标系到世界坐标系的转换公式
		//compute reference direction of flux
		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);
		//计算地理坐标系下的磁场矢量bxyz（参考值）
		bx = sqrtf((hx*hx) + (hy*hy));
		bz = hz; 
		```
		1. 根据四元数矩阵，将地理坐标系下加速度矢量变换到飞行器坐标下（参考值）
		```
		 //estimated direction of gravity and flux (v and w)，下面这个是从世界坐标系到飞行器坐标系的转换公式(转置矩阵)
		//地理坐标系下加速度为(0,0,1g)
		vx = 2*(q1q3 -q0q2);
		vy = 2*(q0q1 +q2q3);
		vz = q0q0 - q1q1 -q2q2 + q3q3;
		```
		1. 将地理坐标系下磁场矢量（参考值）通过四元数矩阵变换到飞行器坐标下
		```
		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);
		```
		1. 将加速度的测量矢量与参考矢量做叉积，把磁场的测量矢量与参考矢量做叉积，得到加速度误差和磁场误差，用于修正陀螺
		```
		// error is sum of cross product between reference direction of fields and direction measured by sensors
		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);
		```
		1. 误差积分，陀螺仪积分
		```
		// integral error scaled integral gain
		exInt = exInt + ex*Ki;
		eyInt = eyInt + ey*Ki;
		ezInt = ezInt + ez*Ki;
		// adjusted gyroscope measurements
		gx = gx + Kp*ex + exInt;
		gy = gy + Kp*ey + eyInt;
		gz = gz + Kp*ez + ezInt;
		```
1. 1. 1. 龙格-库塔法更新四元数
		```
		// integrate quaternion rate and normalise，四元数更新算法
		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;
		
		```
+ 1. 1. 归一化四元数，由四元数计算姿态角
		```
		// normalise quaternion
		norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
		q0 = q0 * norm;       //w
		q1 = q1 * norm;       //x
		q2 = q2 * norm;       //y
		q3 = q3 * norm;       //z
		Pitch = asin(-2*q1*q3 + 2*q0*q2) * 57.3; //俯仰角，绕y轴转动	 
		Roll  = atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1) * 57.3; //滚动角，绕x轴转动
		Yaw   = atan2(2*q1*q2 + 2*q0*q3,-2*q2*q2 - 2*q3*q3 + 1) * 57.3;  //偏航角，绕z轴转?
		
		```![]()

四元数矩阵

1. 利用扩展卡尔曼滤波器对加速度计和磁力计的观测值来修正陀螺积分得到的姿态角 


1. 在这后面还有位置控制和姿态控制![]()
1. 姿态控制要实现实时精确控制四旋翼飞行器的飞行姿态，首先通过航姿参考系统检测其三个轴向的欧拉角度和角速率，然后对其实施闭环控制，使飞行器达到期望的姿态![]()
1. 位置控制位置回路控制律设计是为了使四旋翼无人飞行器可以快速、准确的跟踪给定轨迹。位置回路控制算法首先需要计算得到所需姿态角度，然后由姿态控制回路计算得出给定姿态角度，实现位置回路跟踪控制。所以，外环为位置环、内环为姿态环![]()

收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。