本篇文章首发于公众号：无人机系统技术。更多无人机技术相关文章请关注此公众号，有问题也可在公众号回复“加群”进入技术交流群进行交流。控制律设计的模型变化

为了公式符号统一，本文左上角的字母表示为所在坐标系，而右下角标r表示为期望值。

之前我们在建模篇中已经对多旋翼飞行器进行了数学建模：

为了便于控制器设计，我们把上面的公式统一到NED坐标系下进行表示：

1.4式子可以进一步表示为：

其中R3向量为旋转矩阵的第三列，也是当前机体轴系z轴单位向量在NED坐标系下的表示。

所以其第三列为：

在小角度假设下有：

那对于水平位置控制，假设飞行器升力约等于重力，则有：

其中：

位置控制器设计

首先我们将位置控制器分成内外环结构，外环控制位置，内环控制速度。这种串级控制器相对于直接控制位置，输出姿态期望的控制器来说有非常明显的优势：

外环控制器设计

对于位置运动学模型：

控制律设计为：

其中，

内环控制器设计

对于位置动力学模型：

平面速度控制律设计为：

其中：

垂直速度控制律设计为：

综上所述，结合制导律输出的偏航角期望，位置控制器的输出为：

外环控制器设计

外环控制器设计与上述相同，对于位置运动学模型：

控制律设计为：

其中，

内环控制器设计

对于位置动力学模型：

控制律设计为：

在这里fr和R3r分别决定的是向量的方向和大小。所以有：

到这里控制律设计的部分就算完成了，但是还有一个工作要做，就是R3r只是旋转矩阵的第三列而已，第一列和第二列哪去了呢？光这样也不能控制姿态啊，所以还要结合期望偏航角来获取旋转矩阵的期望。

对于R3r来说，根据旋转矩阵的定义，它就是机体轴系Z轴的单位向量在NED轴系下的表示，而偏航角的定义是机体轴系X轴在NED坐标系的NE平面的投影与N轴的夹角。所以我们可以得到机体轴系X轴在NE平面投影在NED下的表示为：

将其绕D轴旋转90°后的向量为：

这时候本来应该给大家画一张图的，但是因为画图太费时间了点，所以在这里就不画了，你们可以根据我说的自己画图看会更清晰点。

这上面的几个向量有以下关系：

有的人可能不明白上述这个垂直关系中的第二个是怎么来的，其实很简单，有两个方法，第一个就是将旋转矩阵第一列的表示公式和旋转90°以后的向量作点乘，结果为0，说明是垂直关系。另外一个就是从几何关系来证明了，机体轴系在NE平面的投影，画出投影轴，则投影轴与投影以及机体轴系的X轴在一个平面，投影轴垂直NE平面，则一定垂直旋转90°后的向量，那么旋转90°后的向量即垂直投影轴，又垂直投影向量，就垂直他们的平面，所以也垂直属于此平面的机体轴系X轴。

所以有：

这里要注意的是期望机体轴系的X轴在计算时与Z轴的方向是相关的，因为R3r的第三个元素的正负代表飞行器是否需要翻过来，如果飞行器需要翻过来，那么此时要保持机头方向还是向前需要加负号。

还有一个情况就是R3r的第三个元素为0的时候，这时候说明期望的升力方向在NE平面内，这一般不会出现，如果出现了我们就给期望旋转矩阵的第一列赋值为（0,0,1），避免计算错误就行了，不过这不是一个非常好的方法，我觉得应该根据之前计算出的期望旋转矩阵的方向来修正一个合理的值是更好的方法，这个以后我们有机会再讨论。

自此，旋转矩阵的三个向量都算出结果了。综上所述，位置控制器的输出为：

本节内容主要阐述了位置控制器的设计思路，给出了控制律的推导过程。同时，也分别在小角度假设下和非小角度假设下给出了不同的控制律设计结果。在实际使用中，考虑到飞行器的飞行安全以及控制效果，我建议还是以非小角度假设的设计结果进行控制器的设计为好。

至于外环使用P控制，而内环使用PID控制的原因是外环是简单的一阶运动学模型，使用P控制就能达到比较好的控制效果，而内环易受扰动影响，所以使用PID控制可以达到抗扰的目的，在实际使用中由于D项易受噪声干扰，因此也常用PI控制器进行控制。

偏航角期望一般由制导律直接给出，所以在控制器设计时我们默认为已知。为避免大家迷惑，特此说明。