从动力学建模和几个四旋翼核心算法角度分析半自主飞控系统的建立,即实现传统四旋翼的姿态控制和高度控制的过程,文章主要借鉴了北航多旋翼设计课程、正点原子minifly微型四旋翼的资料、《四旋翼无人飞行器设计》清华出版社,代码示例来自正点原子。但是吧,后两个资料在理论部分都有点东拼西凑的感觉,有些错误,看起来很伤…总之,本文多处为个人理解,如有错误,感谢指出。
目录
一、相关理论知识
1.坐标系与欧拉角
进行动力学建模之前首先建立坐标系,在此建立地球坐标系和机体坐标系,如图所示,这里地球系z轴方向向下指向地心,机体系x轴为机头方向。
当描述一个三维空间内的刚体转动时,需要选用三个独立的角度来表示刚体的相对位置。即,刚体绕固定点的旋转可以看成是若干次旋转的合成,旋转方法不是唯一的,所以欧拉角有多种取法,不同的取法、不同的转动顺序都会对于不同的旋转矩阵。在研究四旋翼时,为了与四旋翼的滚转、俯仰、偏航相对应,可以取绕x旋转角度为φ(滚转,右滚为正),绕y轴旋转角度为θ(俯仰,仰头为正),绕z轴旋转为ψ(偏航,右偏为正)。
由地球坐标系转到机体坐标系为xyz旋转,又称卡尔丹角。实际上理论分析时,旋转顺序不是很重要,zyx、zxy都可以,虽然最后姿态解算时四元数与欧拉角的关系式不同,但是都可以进行解算。
2.旋转矩阵
绕x轴旋转的旋转矩阵为,
绕y轴旋转的旋转矩阵为,
绕z轴旋转的旋转矩阵为,
这样,xyz旋转时,一个向量由地球系的表示转为机体系的表示,可以写成,
由机体系转向地球系的姿态矩阵为,
3.四元数及其与欧拉角的关系
关于四元数的详细定义等参见秦永元的《惯性导航》,非常详细,这里不搬了。
以zyx旋转顺序为例,此时其旋转矩阵