一、准备知识
1.串级PID
PX4中多旋翼姿态控制是采用的串级PID加前馈的复合控制策略。分内外两环,外环只有一个比例环节,内环采用PID加前馈的控制方式。外环主要是由control_attitude()函数负责,由期望姿态(使用四元数表示)和当前姿态(也是使用四元数表示)得到角度差,作用于外环的比例环节(对偏航的处理比较特殊),得到期望角速率,传递给内环;内环主要是由control_attitude_rates()函数负责,由期望角速率和当前角速率得到角速率差,作用于内环的PID环节,同时期望角速率通过前馈通道累加到PID输出的控制量上,得到最终的要传递给混控器的控制量(可以理解为转矩)。此时多旋翼姿态控制模块的使命就基本完成了,接下来(src/drivers/px4fmu/fmu.cpp)由混控器根据这些控制量和混控器矩阵(仅对旋翼而言)进行力矩的分配,然后将分配到各个电机上的力矩换算成PWM值,由px4fmu最终落实到硬件上。
PX4中的串级PID就讲这么多,深入了解请参考其他相关资料。
2.罗德里格旋转公式
3.Roll/Pitch和Yaw解耦控制
首先,简单通俗地介绍一下多旋翼姿态控制的基本原理(以四旋翼为例)。
四旋翼横滚角和俯仰角控制原理:
电机转动产生轴向的升力,对称位置的电机转速不一样,产生升力差,进而产生改变横滚角或俯仰角的转矩。
四旋翼偏航角控制原理:
对称位置的电机转速一样,相邻位置的电机转速不一样,而多旋翼对称位置电机转向一致,相邻位置电机转向相反,这样会产生垂直于轴向的扭矩,进而改变偏航角。
现在,我们进入正题。
为什么要解耦控制?
因为横滚角/俯仰角和偏航角控制方式不一样,横滚角/俯仰角是考靠轴向升力之差产生转矩,而偏航角是靠扭矩,所以它们的响应速度也不一样,偏航角控制的响应速度只有前两者的一半左右,所以分开控制能获得更快的响应速度。
怎么实现解耦控制?
先【主要】Roll和Pitch联动,使当前B系中的Z轴向期望姿态的Z轴靠拢,后【主要】进行Yaw的旋转,即对齐X和Y轴。
实际代码中的实现:给Yaw的旋转加一个范围是[0,1]的权重,它是取的(cos<z,z_sp>)^2(两Z轴之间夹角的余弦值的平方),使得Z轴夹角越大,Yaw旋转的权重越小,反之越大。
4.“轴角法”表示误差
当前姿态可以通过绕一条轴旋转一定的角度来达到期望姿态。用一条旋转轴u(单位向量)和一个旋转角θ来表示旋转的方法就是所谓的“轴角法”,可以表示为方向为u,模为θ的向量 θu = (θ a, θ b, θ c)T。这样乘外环比例系数后直接就可以得到期望机体角速率。(代码中对Roll和Pitch误差采用了这种方法来表示,对Yaw误差则是单独处理,然后附加在误差向量上,差别不大)
二、多旋翼姿态控制模块(mc_att_control)源码剖析(基于1.7.2版本代码)
前面提到控制分了内外两环,外环由control_attitude()函数负责,内环由control_attitude_rates()函数负责,下面就开始剖析这两个函数。
首先外环:
然后内环:
三、最后
PX4多旋翼位置控制就先讲到这里,有没有看得头晕眼花啊?(去年暑假我第一次接触无人机的时候反正是晕了
)
![大笑](http://static.blog.csdn.net/xheditor/xheditor_emot/default/laugh.gif)