b站有视频不错,关于四元数的
https://www.bilibili.com/video/BV1Cs411X7kT
yyj
@yyj
yyj 发布的帖子
-
四元数个人心得分享
前言
四元数是一种可以用来描述物体旋转姿态的数学工具,相比于其他描述方式,四元数不会像欧拉角一样有万向锁问题,且计算方式相对欧拉角和旋转矩阵简洁,总的来说,好用就完事。
网上关于四元数的资料大多只贴四元数的运算规则,然后来一句非常抽象,非常不友好,本篇希望尽量以容易理解的方式介绍四元数。
本篇中提到四元数默认为单位四元数,单位四元数仅表示旋转,非单位四元数还代表放缩
单位四元数即模为1的四元数
关于四元数
表达方式
四元数由四部分组成,一个实部和三个虚部,是一类超复数,其中一种表达形式为 a + bi + cj + dk。
在编程时,一般为以下形式:typedef struct q { float w; float x; float y; float z; } Q;
物理意义
首先,物体在三维空间中的姿态可表示为物体在世界坐标系下沿着某根轴旋转某个角度表示,而这个轴可由一个三维空间中的单位向量确定,因此,只需四个坐标便可表示三维物体的旋转姿态,四元数表示三维旋转的方法就是基于这种思路的。
在世界坐标系Oxyz中,有三维单位向量 alpha(l,m,n) ,将物体从初始位置(即物体的自身坐标系Ox'y'z'与世界坐标系重合的位置)沿着alpha以右手法则(类似安培定则)旋转theta,则此时物体的姿态可用四元数
cos(theta / 2) + (sin(theta/2) * l) i + (sin(theta/2) * m) j + (sin(theta/2) * n) k
表示。运算方式(四元数用q表示)
四元数的模
类似向量的模,即
buf = p_q->w * p_q->w + p_q->x * p_q->x + p_q->y * p_q->y + p_q->z * p_q->z; buf = sqrtf(buf);
|q| = buf;
四元数共轭
类似于复数共轭q的共轭一般表示为q*
void quaternion_Conjugate(Q *p_q) { p_q->x *= -1.0f; p_q->y *= -1.0f; p_q->z *= -1.0f; }
四元数逆
对于非单位四元数q,q的逆q-1 = q* / |q|
对于单位四元数,q-1 == q*
q inverse(q) == inverse(q) q == q0 == (1,0,0,0)四元数单位化
一般使用单位四元数表示旋转姿态,单位化方法如下:
void quaternion_Unitization(Q *p_q) { static float buf; buf = p_q->w * p_q->w + p_q->x * p_q->x + p_q->y * p_q->y + p_q->z * p_q->z; buf = sqrtf(buf); p_q->w /= buf; p_q->x /= buf; p_q->y /= buf; p_q->z /= buf; }
四元数乘法
对于四元数乘法q1q2(四元数乘法不是点积也不是叉积),四元数乘法的物理意义是将位于q1姿态的物体进行q2旋转(相对于q1自身坐标系)
四元数乘法遵循结合律,即 q1q2q3 == q1(q2q3),但不遵循交换律,即q1q2 不一定等于 q2q1
对于四元数乘法q3 = q1q2,公式如下:void quaternion_Multiplication(Q *p_q1, Q *p_q2, Q *p_q3) { p_q3->w = (p_q1->w * p_q2->w) - (p_q1->x * p_q2->x) - (p_q1->y * p_q2->y) - (p_q1->z * p_q2->z); p_q3->x = (p_q1->w * p_q2->x) + (p_q1->x * p_q2->w) + (p_q1->y * p_q2->z) - (p_q1->z * p_q2->y); p_q3->y = (p_q1->w * p_q2->y) - (p_q1->x * p_q2->z) + (p_q1->y * p_q2->w) + (p_q1->z * p_q2->x); p_q3->z = (p_q1->w * p_q2->z) + (p_q1->x * p_q2->y) - (p_q1->y * p_q2->x) + (p_q1->z * p_q2->w); }
四元数除法
q3 = q2/q1 = inverse(q1) q2
其实并没有严格的对于四元数除法的定义,单纯是因为之前有用到四元数求相对姿态方便起见起的名字,除法是相对于乘法而来,物理意义为两者之间的相对姿态(q1经过q3旋转得到q2),更准确来说,是q2相对于q1在q1自身坐标系中的姿态
代码如下:void quaternion_Division(Q *p_q1, Q *p_q2, Q *p_q3) { quaternion_Conjugate(p_q1); quaternion_Multiplication(p_q1, p_q2, p_q3); quaternion_Conjugate(p_q1); }
在空间中旋转一个点(旋转方向)
对于空间中某点p(r,s,t),将该点(方向)进行q旋转,可用四元数 h(0,r,s,t),与q进行如下运算
qhq*
得到的即为旋转之后的方向(结果的q.w必定为0)用四元数计算相对姿态
需要用两个陀螺仪进行姿态补偿,即,将两陀螺仪固定在一起,转动,得到的相对姿态应该不变
使用之前提到的四元数除法即可从四元数到欧拉角
欧拉角有内旋外旋,每种各有 3 x 2 x 2 = 12种,总计24种,解算方法也有24种,常用为zyx顺序内旋欧拉角,解算方法如下
void quaternion_To_Euler(Q *p_q, Euler *p_e) { p_e->yaw = atan2f(2 * (p_q->w * p_q->z + p_q->x * p_q->y), 1 - 2 * (p_q->z * p_q->z + p_q->y * p_q->y)) / PI * 180; p_e->pitch = asinf(2 * (p_q->w * p_q->y - p_q->x * p_q->z)) / PI * 180; p_e->roll = atan2f(2 * (p_q->w * p_q->x + p_q->y * p_q->z), 1 - 2 * (p_q->y * p_q->y + p_q->x * p_q->x)) / PI * 180; }
yaw为+-180,pitch为+-90,roll为+-180
如有错误,欢迎指正,欢迎交流
-
RE: yyj的STM32学习笔记
task3
任务描述:
完成单个按键状态检测,能够利用指示灯识别单击、双击、长按
解析
- 按键按下状态的检测不能直接使用中断,由于机械按键会在按下、抬起时产生抖动,会触发多次中断,故需要进行消抖处理,抖动一般在10ms以内,本次使用延时消抖,即在检测到按下或抬起后在一段时间后再次检测,确认其确实发生了按下或者抬起
- 按键检测以时间片的原则进行,使用TIM6计时器每隔10ms检测一次按键状态,使用两个变量分别储存按下、抬起的时间
- 使用一个变量记录所处的状态,当检测到按键状态发生变化时,根据现在所处的状态和变量的值进行动作
- 本次设定为2s以下为单击,2s以上为长按,第一次单击后1s内再次单击为双击
代码
由于代码由cube生成,比较繁琐,故只贴自己写的部分
宏
#define MAX 999 #define TIME_CLICK_PRESS 200 //点击和长按分界线 #define TIME_WAIT 100 #define FLAG_PLAIN 0 //普通状态 #define FLAG_FIRST_HIT 1 //第一次被按下 #define FLAG_WAIT_SECOND_HIT 2//等待第二次被按下 #define FLAG_SECOND_HIT 3 //第二次被按下 #define KEY_TIME_INCREASE(x) (((x) >= MAX) ? MAX : ((x) + 1)) //代替自增操作,防止溢出,对变量的值做限制
变量
uint32_t POSITIVE = 0, NEGATIVE = 2; //按下、抬起时间 uint16_t FLAG = FLAG_PLAIN; //状态
主要函数
uint16_t if_KeyHit(void) //检测按键状态 { if(GPIOA->IDR & 0x1) { return 0; } else { return 1; } } void TimeSlicer(void) //管理时间变量和状态转换 { if (if_KeyHit()) //检测到按键按下 { if (POSITIVE > 1) //正常继续按下状态 { POSITIVE = KEY_TIME_INCREASE(POSITIVE); StateSwitch(); NEGATIVE = 0; } else if (POSITIVE == 1) //确认切换为按下 { POSITIVE = 2; StateSwitch(); NEGATIVE = 0; } else //处于抬起状态,需确认是否按下 { NEGATIVE = KEY_TIME_INCREASE(NEGATIVE); POSITIVE = 1; } } else //检测到按键抬起 { if (NEGATIVE > 1) //正常继续抬起状态 { NEGATIVE = KEY_TIME_INCREASE(NEGATIVE); StateSwitch(); POSITIVE = 0; } else if (NEGATIVE == 1) //确认抬起状态 { NEGATIVE = 2; StateSwitch(); POSITIVE = 0; } else //处于按下状态,需确认是否抬起 { POSITIVE = KEY_TIME_INCREASE(POSITIVE); NEGATIVE = 1; } } } void StateSwitch(void) //状态操作和转换 { switch (FLAG) { case FLAG_PLAIN: if(if_KeyHit()) //按键按下 { FLAG = FLAG_FIRST_HIT; } break; case FLAG_FIRST_HIT: if(!if_KeyHit()) //按键松开 { if (POSITIVE > TIME_CLICK_PRESS) //长按 { FLAG = FLAG_PLAIN; } else //第一次为点击 { FLAG = FLAG_WAIT_SECOND_HIT; State_0(); } } else { if(POSITIVE > TIME_CLICK_PRESS) { State_3(); } } break; case FLAG_WAIT_SECOND_HIT: if(!if_KeyHit()) //按键没有按下 { if (NEGATIVE > TIME_WAIT) //等待时间已过,为单击 { State_1(); FLAG = FLAG_PLAIN; } } else //按键按下 { FLAG = FLAG_SECOND_HIT; } break; case FLAG_SECOND_HIT: if(!if_KeyHit()) //按键抬起 { if(POSITIVE < TIME_CLICK_PRESS) //第二次为点击 { State_2(); FLAG = FLAG_PLAIN; } else //第二次为长按 { State_1(); State_3(); FLAG = FLAG_PLAIN; } } break; default:; } } void State_0(void) //用LED灯指示检测到的按键操作类型 { GPIOF->ODR |= (0x1 << 8); GPIOF->ODR |= (0x1 << 9); } void State_1(void) { GPIOF->ODR &= ~(0x1 << 8); GPIOF->ODR |= 0x1 << 9; } void State_2(void) { GPIOF->ODR |= 0x1 << 8; GPIOF->ODR &= ~(0x1 << 9); } void State_3(void) { GPIOF->ODR &= ~(0x1 << 8); GPIOF->ODR &= ~(0x1 << 9); }
总结
这次开始用的板子有点问题,导致找了两天bug也没找出来,最后换了块板子,效果还不错,硬件的问题太折磨人了555
-
RE: yyj的STM32学习笔记
task2
任务描述:
- 编写按键程序,按下时LED亮,抬起时LED灭
- 使用PWM波驱动舵机
解析
- 按键程序最初想法是在主函数的while(1)循环中扫描GPIO的IDR寄存器,不过后来的想法是在定时器中断里每隔一段时间(如10ms)扫描一次IDR寄存器
- PWM波形使用通用定时器进行输出,每个通用定时器有四个通道,通过将CCRx的值与CNT的值做比较,输出高低电平,通过控制频率和占空比即可控制舵机
- 本次使用的舵机型号为SG90相关参数如下:
- 本次使用STM32CubeMX进行配置和初始化,配置如下:
-
运放电路分析(一)
回复: 运放入门总结
1.同相放大电路
通过负反馈,最终输出电压Vo稳定在一个值
由虚短得Vp=Vn
由虚断可得,流经R1和R2的电流i1=i2=iR
则最终输出的电压Vo=(1 + R2/R1)Vi2.反相放大电路
通过负反馈,虚短成立,最终输出电压Vo稳定在一个值
由虚短可得,Vp=Vn=0
由虚断可得,i1=i2
则最终输出的电压Vo=-(R2/R1)Vi3.放大电路的其他应用
求差电路
由虚短得
Vp=Vn
由虚断得
i2=i3
i1=i4
由分压原理得
ip=Vi2(R3/(R2+R3))
in=Vi1+(R1/(R1+R4))(Vo-Vi1)
联立解得
Vo=(1+R4/R1)(R3/(R2+R3))Vi2-(R4/R1)Vi1
若R4/R1=R3/R2,则
Vo=(R4/R1)(Vi2-Vi1)求和电路
由虚断得
i1+i2=i3
由虚短得
Vp=Vn=0i1=-Vi1/R1
i2=-Vi2/R2联立得
Vo1=-((R3/R1)Vi1+(R3/R2)Vi2)
若R1=R2=R3
则Vo1=-(Vi1+Vi2)
在Vo1后加一个放大倍数为1的反相放大器,则
vo=Vi1+Vi2积分电路
由虚短得
Vp=Vn=0
由虚断得
i1=i2=ii1=Vi/R
微分电路
由虚短得Vp=Vn=0
由虚断得i1=ii=C(dVi/dt)
Vo=-iR
联立得
-
RE: yyj的STM32学习笔记
- 错别字,以后会在发布前看一遍,并且打算专门开一个运放专题新帖
- 推挽输出和开漏输出相比,能真正地输出高电平,而开漏输出需要借助上拉电阻
- 在配置时钟树时,定时器的周期为72MHz,在本例中,将预分频器(PSC)设置为71,对定时器进行72分频,即1MHz,将自动重装载器(ARR)设置为999,即每1000个周期产生一次中断,则最终产生中断的频率为1KHz,周期为1ms
- 遇到的问题大多数是工具的应用方面的,对于一个stm32项目的创建、初始化、调试、下载,刚开始时会因为遗漏某些细节而导致错误,而这种错误往往是不容易排查的
比如,刚开始时由于没有配置时钟树导致程序不能正常运行,编译程序时由于缺少某些文件(.s之类的)而导致编译出错等等
除此之外,还有一些不够细心导致的错误
比如,开始焊放大器时,由于线路规划时粗心,画错了一个电阻的位置,导致出错