转载自:https://blog.csdn.net/wxc971231/article/details/97449026
https://blog.csdn.net/luoshi006/article/details/51513580
一、姿态解算原理相关
1、简介
- 对于四旋翼无人机来说,有两个相关坐标系,即地理坐标系
OnXnYnZnOnXnYnZnOnXnYnZn和机体坐标系
ObXbYbZbObXbYbZbObXbYbZb。
- 姿态角的检测对于四旋翼控制是至关重要的。比如在做飞行器姿态控制的时候,通常使用串级pid的方法,需要得到飞行器姿态角(即欧拉角)来做角度位置环反馈。再如利用装置在机腹垂直向下拍摄的摄像头,进行机体视觉定位时,也需要靠姿态角进行视觉反馈补偿。
- 要解算姿态角,就要研究从地理坐标系到机体坐标系的转换过程。这个转换过程不是唯一的,比如可以先绕X轴旋转
θθθ,再绕Y轴旋转
γγγ,最后绕Z轴旋转
ψψψ,这样得到的一组姿态角称为卡尔丹角;也可以按
ZYX
的顺序旋转,这样得到的一组姿态角称为欧拉角(这是最常用的,pitch
,roll
,yaw
)。不管按照什么顺序,得到的角度都可以称作广义欧拉角。实际理论分析时,旋转顺序不是很重要,这个顺序会影响四元数与欧拉角的关系,但是都可以进行解算。
2、坐标变换和旋转矩阵
下面从最基本的坐标变换入手开始讲解,请看如下坐标系旋转:
OAXAYAZAOAXAYAZAOAXAYAZA绕OX旋转
θθθ到
OBXBYBZBOBXBYBZBOBXBYBZB
对于原系
OAXAYAZAOAXAYAZAOAXAYAZA中的一个向量
[rxA,ryA,rzA][rxA,ryA,rzA][rxA,ryA,rzA],转换到新系
OBXBYBZBOBXBYBZBOBXBYBZB中的向量
[rxB,ryB,rzB][rxB,ryB,rzB][rxB,ryB,rzB],有:
rxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzArxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzArxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzA
可以用旋转矩阵表示
[rxBryBrzB]=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)][rxAryArzA]⎣⎡rxBryBrzB⎦⎤=⎣⎡100amp;0amp;cos(θ)amp;−sin(θ)amp;0amp;sin(θ)amp;cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤⎣⎡rxBryBrzB⎦⎤=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤
-
中间的就是 绕x轴旋转
θθθ 的旋转矩阵,记作
Cx(θ)=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)]Cx(θ)=⎣⎡100amp;0amp;cos(θ)amp;−sin(θ)amp;0amp;sin(θ)amp;cos(θ)⎦⎤Cx(θ)=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤
-
同样的,还有 绕y轴旋转
γγγ 的旋转矩阵,记作
Cy(γ)=[cos(γ)0−sin(γ)010sin(γ)0cos(γ)]Cy(γ)=⎣⎡cos(γ)0sin(γ)amp;0amp;1amp;0amp;−sin(γ)amp;0amp;cos(γ)⎦⎤Cy(γ)=⎣⎡cos(γ)0sin(γ)010−sin(γ)0cos(γ)⎦⎤
-
同样的,还有 绕z轴旋转
ψψψ 的旋转矩阵,记作
Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001]Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0amp;−sin(ψ)amp;cos(ψ)amp;0amp;0amp;0amp;1⎦⎤Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎦⎤
-
假如现在我们按ZXY
的顺序,从地理坐标系
OnXnYnZnOnXnYnZnOnXnYnZn旋转到机体坐标系
ObXbYbZbObXbYbZbObXbYbZb,整体旋转矩阵为
Cnb=Cy(γ)Cx(θ)Cz(ψ)=[cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)−sin(ψ)cos(θ)sin(ψ)cos(θ)cos(ψ)cos(θ)sin(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)cos(γ)cos(θ)]Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)amp;−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)amp;cos(ψ)cos(θ)amp;−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)amp;−sin(ψ)cos(θ)amp;sin(θ)amp;cos(γ)cos(θ)⎦⎤Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)cos(ψ)cos(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)−sin(ψ)cos(θ)sin(θ)cos(γ)cos(θ)⎦⎤
-
注意一点,如果从n系到b系的旋转矩阵为
CnbCnbCnb,则从b系到n系的旋转矩阵为
CnbTCnbTCnbT,显然有
Cnb∗CnbT=ECnb∗CnbT=ECnb∗CnbT=E,所以旋转矩阵是正交矩阵
-
这个结论不能直接使用,一方面这个直接是不可解的,另一方面大量三角函数运算会导致大量资源占用。
3、四元数
- 四元数是一种超复数,由四个元构成:
Q(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3k
- 关于四元数更详细的介绍可以参考四元数
- 利用四元数我们可以提出另一种描述空间矩阵的方法,具体可以参考秦永元的《惯性导航》和【教程】四旋翼飞行器姿态解算算法入门学习-Rick Grimes
(1)四元数和欧拉角的关系
- 注意这三个式子中,
θθθ是绕Y轴转角,
ϕϕϕ是绕X轴转角,
ψψψ是绕Z轴转角,和前文有所不同
- 按
ZYX
的顺序从地理坐标系
OeXeYeZeOeXeYeZeOeXeYeZe旋转到机体坐标系
ObXbYbZbObXbYbZbObXbYbZb,整体旋转矩阵为
- 对应的四元数表示法为:
- 解算出的欧拉角为
(2)四元数的求解
- 飞行器姿态的改变,可以对应到旋转矩阵的改变,进一步对应到四元数的改变。实时姿态计算,实际上也就是实时更新四元数。
- 我们可以构建四元数关于时间的微分方程,来研究四元数关于时间的变化规律,求解该微分方程,即可解出四元数
1、建立微分方程
- 从四元数的三角表示法入手推到微分方程,具体推到过程可以参考秦永元的《惯性导航》
Q=cos(θ)2+λsin(θ)2Q=2cos(θ)+2λsin(θ)Q=2cos(θ)+2λsin(θ)
dQdt=1/2[0−ωx−ωy−ωxωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]⋅[q0q1q2q3]dtdQ=1/2⎣⎢⎢⎡0ωxωyωzamp;−ωxamp;0amp;−ωzamp;ωyamp;−ωyamp;ωzamp;0amp;−ωxamp;−ωxamp;−ωyamp;ωxamp;0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤dtdQ=1/2⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωx−ωyωx0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
记为dQdt=Φ⋅Q记为dtdQ=Φ⋅Q记为dtdQ=Φ⋅Q
2、一阶龙格库塔法求解微分方程
一阶龙格库塔法是求解微分方程常用的工程方法,原理是把微分转化为微元增量,利用递推迭代的方法求解
-
设有微分方程
dydx=f(x,y)dxdy=f(x,y)dxdy=f(x,y)
求解y的迭代公式为
y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))
-
对应到四元数微分方程:
Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)
整理得
[q0q1q2q3]t+Δt=[q0q1q2q3]t+12⋅Δt⋅[−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0]⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤
这里
ωωω就是三轴角速度,可以用陀螺仪直接测得
4、旋转矩阵中的一列
- 旋转矩阵中的一列有特殊意义,它代表原坐标系中一个方向的单位向量(X/Y/Z),在新坐标系中对应的向量值
- 用方向向量(如X方向
[1,0,0]T[1,0,0]T[1,0,0]T)左乘旋转矩阵可以轻易看出,这里不写了
二、匿名四轴姿态解算分析
按Ano_Imu.c
文件顺序进行分析
1、坐标系定义
- 匿名四轴姿态解算部分主要写在
Ano_Imu.c
中
- 匿名四轴程序中一共有三个坐标系:
- 地理坐标系,标记为w(官方定义如下)
- 机体坐标系,标记为a(官方定义如下)
- 航向坐标系,标记为h(专门解释一下,四旋翼飞行时,姿态角pitch和roll一般很小,在任意时刻,如果我们粗略地认为飞行器始终与底面平行(把飞机摆正,使pitch=roll=0),此时的机体坐标系即为航向坐标系)
2、坐标系转换
(1)匿名旋转坐标系
- 匿名的坐标系变换,是按是按
ZYX
顺序从地理坐标系转到机体坐标系的,这个旋转矩阵是前面提过的 (注意这三个式子中,
θθθ是绕Y轴转角,
ϕϕϕ是绕X轴转角,
ψψψ是绕Z轴转角)
Cwa=[cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)]Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)amp;sin(ψ)cos(θ)amp;sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)amp;cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)amp;−sin(θ)amp;sin(ϕ)cos(θ)amp;cos(ϕ)cos(θ)⎦⎤Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)⎦⎤
- 程序中用
二维数组att_matrix
表示从机体坐标系转到地理坐标系的旋转矩阵,是上面那个的转置
attmatrix=Caw=(Cwa)T=[cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)]att_matrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)amp;sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)amp;sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)amp;sin(ϕ)cos(θ)amp;cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)amp;cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)amp;cos(ϕ)cos(θ)⎦⎤attmatrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)⎦⎤
- 注意这里绕Y轴转角
θθθ (pitch),绕X轴转角
ϕϕϕ (roll)都是很小的角
(2)相关代码分析
#