UAV021（七）：控制系统设计
目录
- 序
- 一、控制原理
- 1.1 控制系统框图
- 1.2 姿态控制器
- 1.2.1 串级PID
- 1.2.2 PID参数及意义
- 1.3 控制分配
- 二、程序设计
- 2.1 PID结构体
- 2.2 PID
- 2.3 任务函数与API接口
序
不知道该怎么开始，写得随意些吧。之前做了许多工作，总结起来包括：读取并校准九轴传感器数据、姿态估计、PWM控制电机、捕获遥控器指令。如果把无人机看作一个黑箱，那么输入是遥控指令，具体说是给出期望的姿态角和油门；输出是电机转速，具体就是PWM波的占空比。再用九轴传感器作为反馈，形成闭环。因此，系统输入来自于遥控器和传感器，系统输出为电机PWM波占空比，如下图：
黑箱要做的，就是将输入输出联系起来，将目标姿态、当前姿态与PWM占空比连接起来。
如何连接呢，此节一一道来。
一、控制原理
1.1 控制系统框图
将黑箱展开，就是虚线里的内容。其中， Θ d = [ θ d , ϕ d , ψ d ] \Theta_d=[\theta_d, \phi_d, \psi_d] Θd=[θd,ϕd,ψd]为目标姿态角（依次为俯仰角、滚转角和偏航角）， Θ m = [ θ m , ϕ m , ψ m ] \Theta_m=[\theta_m, \phi_m, \psi_m] Θm=[θm,ϕm,ψm]为传感器测量融合得到的姿态角。 ω m = [ ω m x , ω m y , ω m z ] \omega_m=[\omega_{mx}, \omega_{my}, \omega_{mz}] ωm=[ωmx,ωmy,ωmz]为测量得到的角速度。 τ d = [ τ d x , τ d y , τ d z ] \tau_d=[\tau_{dx}, \tau_{dy}, \tau_{dz}] τd=[τdx,τdy,τdz]力矩， f d f_d fd为期望的拉力。 σ k ( k = 1 , 2 , 3 , 4 ) \sigma_k(k=1,2,3,4) σk(k=1,2,3,4)为最终给四个电机的占空比。
1.2 姿态控制器
1.2.1 串级PID
姿态控制器采用了串级PID，将角度环和角速度环串联在一起。
跟上面一样， Θ d = [ θ d , ϕ d , ψ d ] \Theta_d=[\theta_d, \phi_d, \psi_d] Θd=[θd,ϕd,ψd]为目标姿态角（依次为俯仰角、滚转角和偏航角）， Θ m = [ θ m , ϕ m , ψ m ] \Theta_m=[\theta_m, \phi_m, \psi_m] Θm=[θm,ϕm,ψm]为传感器测量融合得到的姿态角。 ω m = [ ω x m , ω y m , ω z m ] \omega_m=[\omega_{xm}, \omega_{ym}, \omega_{zm}] ωm=[ωxm,ωym,ωzm]为测量得到的角速度， ω d = [ ω x d , ω y d , ω z d ] \omega_d=[\omega_{xd}, \omega_{yd}, \omega_{zd}] ωd=[ωxd,ωyd,ωzd]为外环得到的期望角速度。
外环也叫做角度环，姿态环；内环叫做角速度环。上面串级PID从三个轴分别看就会简单很多。俯仰角PID是与绕y轴PID串联的（飞机绕y轴转动是俯仰角变化），滚转角PID是与绕x轴PID串联的，偏航角PID与绕z轴PID串联。
外环算法为：
如果矩阵太复杂，展开看看也无妨：
注意上面每个 K p , K i , K d K_p, K_i, K_d Kp,Ki,Kd 不一定相同，只是为了简洁去除下标。
内环算法矩阵形式及展开如下：
和上面一样， K p , K i , K d K_p, K_i, K_d Kp,Ki,Kd 不一定相同，只是为了简洁去除下标。
不要被几个公式唬住，只是是三个轴看起来多点而已，仅仅是两级串联而已，每个公式都仅仅是一个PID而已。
1.2.2 PID参数及意义
上面这张图可以说很经典了，相信你也了解（念gai），需要做的就是调节Kp、Ki和Kd三个参数，得到一个从输入到输出的映射。对于我们的串级PID，就是需要调节两组PID参数，实现从角度到角速度的映射（当然不要想着直接差分，实际上是经过PID映射后再差分），再从角速度到力矩的映射。
说点题外话，我们能够直观些感受PID吗？智者见智吧，目前，个人觉得从频域思考，积分I 是低通滤波，想想低频（低到直流分量）积分，会一直累加，高频积分会被消灭。差分D 则是高通滤波器，低频分量一差分就消耗殆尽了。如此看来，比例P 就很友好，对低频高频一视同仁，调节它就是调节步长，步长小，容易稳定地达到目标，但是慢，反之则快而不稳。PID要做的就是要实现快速稳定地达到目标值。
那到底怎么调节PID呢？我们先不管，时机还不成熟，后面我们会真刀真枪地上场的。现在会写出PID公式并且编程即可。
1.3 控制分配
控制分配是实现四个电机转速配合。我们已经通过PID产生合适的力矩，通过遥控器给出合理的拉力（油门产生），三轴的力矩与向上的拉力如何分配给四个电机PWM，这就是控制分配完成的事情。值得注意的是，这里拉力和力矩并不具有真正物理意义，实质上已经是PWM占空比的单位了。规定右上角电机为1号，逆时针编号；规定各个电机按下图转动为正方向，如下图：
首先看拉力，当油门上推时，各个电机按照上图的方向加速转动，产生向上升力。此时各个电机转动皆为正方向（用1表示），可以得到控制分配矩阵最后一列全为1，横杠表示不关心。
再来看俯仰角，假设俯仰角要增大，也即飞机要上仰。明显地，此时1号2号电机要加速转动，3号4号电机减速转动，而且增减一致（依旧再小角度假设条件下），这样向上的升力是不变的。飞机上仰，是绕y轴转动，因此与 τ y \tau_y τy 有关，控制分配矩阵现在为：
对于滚转角，滚转角增大时，是无人机绕x轴逆时针转动，与 τ x \tau_x τx 有关，此时2号3号电机加速转动，1号4号电机减速转动。控制分配矩阵如下：
最后看看偏航角，无人机顺时针偏转为正方向。此时，1号3号电机加速转动，产生较大反扭力矩，2号4号电机减速转动，反扭力矩较小。与 τ z \tau_z τz 有关，控制分配矩阵如下：
至此，完成从力与力矩到PWM波占空比的映射。所以，关键还是前面的PID参数，以及遥控器油门的映射结果，它们决定电机占空比大小。
二、程序设计
把以上公式用计算机语言描述出来，便是此节的内容。其实，程序设计时还需要考虑一些实际问题，因为我们不是做理论，而是要真刀真枪地干的。实际中有位置式PID和增量式PID，此处采用位置式PID，先试一下。另外便是限幅，常见的有对PID积分进行限幅，对PID输出结果限幅。请看程序：
2.1 PID结构体
个人觉得看程序的是时候喜欢看到整个文件，而不是文件中的一小部分，这样有利于实际运行。因此，虽然只是说明PID结构体，但依旧给出整个头文件。
// control.h
#ifndef CONTROL_H
#define CONTROL_H
#include "attitude.h"
/* 位置式PID结构体 */
struct PID_t
{
float Kp;
float Ki;
float Kd;
float e_now; // 此次误差
float e_last; // 上次误差
float e_sum; // 误差积分
float e_delta; // 误差差分
};
/* PID控制器与动力分配 */
void LimitRangeFloat(float *num, const float nmin, const float nmax); // 浮点数限幅
void LimitRangeUint16(uint16_t *num, const uint16_t nmin, const uint16_t nmax); // 16位无符号数限幅
struct DOF3_t GetDesireAngleRate(const struct ATTI_t atti_d, const struct ATTI_t atti_m); // 外环PID，根据姿态获取期望角速度
struct DOF3_t GetForceMoment(const struct DOF3_t angle_rate_d, const struct DOF3_t angle_rate_m); // 内环PID，根据角速度获取力矩
void ControlAllocation(const struct DOF3_t force_moment, const float force); // 控制分配
float GetThrForce(float thr); // 根据油门获取升力
void ControlUavTask(void *arg); // 控制任务
/* 设置姿态与角速度 PID 接口 */
void SetPitchPidApi(float Kp, float Ki, float Kd); // 设置俯仰角 PID 接口
void SetRollPidApi(float Kp, float Ki, float Kd); // 设置滚转角 PID 接口
void SetYawPidApi(float Kp, float Ki, float Kd); // 设置偏航角 PID 接口
void SetXarPidApi(float Kp, float Ki, float Kd); // 设置绕 x 轴角速度PID接口
void SetYarPidApi(float Kp, float Ki, float Kd); // 设置绕 y 轴角速度PID接口
void SetZarPidApi(float Kp, float Ki, float Kd); // 设置绕 z 轴角速度PID接口
void SetPidApi(char* name, float Kp, float Ki, float Kd); // 设置 PID 接口，根据 name 区分设置以上几个 PID
void GetPidApi(float *pid); // 获取PID数值
void GetForceApi(float *force); // 获取力与力矩
#endif
PID结构体除了定义三个系数，还定义了几个误差，主要是因为涉及迭代，这样方便些。
2.2 PID
对于串级PID，分别实现外环PID和内环PID，按照公式实现，注意输入与输出的定义即可；对于控制分配，也是按照公式实现的，只不过是把矩阵直接展开。与公式不同的是，此处加入限幅，同时，控制分配时加入一个判断，需要使用到遥控器，遥控器通道5达到相应档位才使用计算结果，否则电机停转，这样可以避免突然失控（调试过程中很可能会出现）。
注意一些语法细节，限幅函数，需要限幅的数据需要使用指针，才能修改值；限制的区间声明为 const，说明不会改变。
// control.c
#include "control.h"
#include "sys.h"
#include "attitude.h"
#include "sbus.h"
#include "pwm.h"
#include "delay.h"
#include 2.3 任务函数与API接口
任务函数就是把以上几个模块组合在一起，这真是令人激动的时刻。
/* 控制无人机任务 */
/* 整个无人机系统输入为遥控器数据--提供姿态角及拉力，传感器测量数据--测量姿态及姿态速率 */
/* 输出为4路PWM波占空比--调节电机占空比 */
/* 中间主要包括 串级PID 和 控制分配 */
void ControlUavTask(void *arg)
{
struct MC6C_t mc6c; // mc6c 遥控器结构体
struct ATTI_t atti_d;
struct ATTI_t atti_m;
struct DOF3_t angle_rate_d; // 期望角速度
struct DOF3_t angle_rate_m; // 测量角速度
struct DOF3_t force_moment; // 根据遥控器输入（期望姿态）最终得到的力矩
float force; // 遥控器（油门）输入产生的力
SetMotorDutyApi(MOTOR1, 400); // 设置电机占空比，初始默认最低，400
SetMotorDutyApi(MOTOR2, 400);
SetMotorDutyApi(MOTOR3, 400);
SetMotorDutyApi(MOTOR4, 400);
while (1)
{
GetMc6cDataApi(&mc6c); // 获取 MC6C 遥控器数据
atti_d.theta = mc6c.ele; // 期望俯仰角
atti_d.phi = mc6c.ail; // 期望滚转角
atti_d.psi = mc6c.rud; // 期望偏航角
GetAttiApi(&atti_m); // 获取测量姿态
angle_rate_d = GetDesireAngleRate(atti_d, atti_m); // 获取期望角速度
GetAngleRateApi(&angle_rate_m); // 获取测量的角速度
force_moment = GetForceMoment(angle_rate_d, angle_rate_m); // 获取力矩
force = GetThrForce(mc6c.thr); // 获取升力
ControlAllocation(force_moment, force); // 控制分配
delay_ms(50);
// 将数据赋值到全局变量，debug需要
force_[0] = force_moment.x;
force_[1] = force_moment.y;
force_[2] = force_moment.z;
force_[3] = force;
// 打印信息
printf("\r\n===================================\r\n");
printf("atti_d: %f, %f, %f \r\n", atti_d.theta, atti_d.phi, atti_d.psi);
printf("atti_m: %f, %f, %f \r\n", atti_m.theta, atti_m.phi, atti_m.psi);
printf("angle_rate_d: %f, %f, %f\r\n", angle_rate_d.x, angle_rate_d.y, angle_rate_d.z);
printf("angle_rate_m: %f, %f, %f\r\n", angle_rate_m.x, angle_rate_m.y, angle_rate_m.z);
printf("force_moment: %f, %f, %f\r\n", force_moment.x, force_moment.y, force_moment.z);
printf("force: %f\r\n", force);
}
}
delay_ms(50) 后面的内容只是debug需要，后期可删除。
最后是一些接口函数，这次定义很多接口，主要是为下一次——Debug做准备。先下集预告一下，下次将实现一个简单的debug系统，方便进行最终的参数调试。
/*********************************** PID设置接口 *****************************************/
/* 设置俯仰角PID接口 */
void SetPitchPidApi(float Kp, float Ki, float Kd)
{
pid_pitch_.Kp = Kp;
pid_pitch_.Ki = Ki;
pid_pitch_.Kd = Kd;
}
/* 设置滚转角PID接口 */
void SetRollPidApi(float Kp, float Ki, float Kd)
{
pid_roll_.Kp = Kp;
pid_roll_.Ki = Ki;
pid_roll_.Kd = Kd;
}
/* 设置偏航角PID接口 */
void SetYawPidApi(float Kp, float Ki, float Kd)
{
pid_yaw_.Kp = Kp;
pid_yaw_.Ki = Ki;
pid_yaw_.Kd = Kd;
}
/* 设置绕x轴角速度PID接口 */
void SetXarPidApi(float Kp, float Ki, float Kd)
{
pid_xar_.Kp = Kp;
pid_xar_.Ki = Ki;
pid_xar_.Kd = Kd;
}
/* 设置绕x轴角速度PID接口 */
void SetYarPidApi(float Kp, float Ki, float Kd)
{
pid_yar_.Kp = Kp;
pid_yar_.Ki = Ki;
pid_yar_.Kd = Kd;
}
/* 设置绕x轴角速度PID接口 */
void SetZarPidApi(float Kp, float Ki, float Kd)
{
pid_zar_.Kp = Kp;
pid_zar_.Ki = Ki;
pid_zar_.Kd = Kd;
}
/* 设置PID接口 */
/* 采用三个字母比较 */
/* pit : pitch */
/* rol : roll */
/* yaw : yaw */
/* xar : x angle rate */
/* yar : y angle rate */
/* zar : z angle rate */
void SetPidApi(char* name, float Kp, float Ki, float Kd)
{
if (memcmp(name, "pit", 3) == 0) // 按字节比较。0: 两字节相等，>0：前面字节大，<0：后面字节大
{
SetPitchPidApi(Kp, Ki, Kd);
printf("Set pit: %.3f, %.3f, %.3f\r\n", Kp, Ki, Kd);
}
else if (memcmp(name, "rol", 3) == 0)
{
SetRollPidApi(Kp, Ki, Kd);
printf("Set rol: %.3f, %.3f, %.3f\r\n", Kp, Ki, Kd);
}
else if (memcmp(name, "yaw", 3) == 0)
{
SetYawPidApi(Kp, Ki, Kd);
printf("Set yaw: %.3f, %.3f, %.3f\r\n", Kp, Ki, Kd);
}
else if (memcmp(name, "xar", 3) == 0)
{
SetXarPidApi(Kp, Ki, Kd);
printf("Set xar: %.3f, %.3f, %.3f\r\n", Kp, Ki, Kd);
}
else if (memcmp(name, "yar", 3) == 0)
{
SetYarPidApi(Kp, Ki, Kd);
printf("Set yar: %.3f, %.3f, %.3f\r\n", Kp, Ki, Kd);
}
else if (memcmp(name, "zar", 3) == 0)
{
SetZarPidApi(Kp, Ki, Kd);
printf("Set zar: %.3f, %.3f, %.3f\r\n", Kp, Ki, Kd);
}
}
/* 获取PID数值接口 */
void GetPidApi(float *pid)
{
// pitch PID
pid[0] = pid_pitch_.Kp;
pid[1] = pid_pitch_.Ki;
pid[2] = pid_pitch_.Kd;
// roll PID
pid[3] = pid_roll_.Kp;
pid[4] = pid_roll_.Ki;
pid[5] = pid_roll_.Kd;
// yaw PID
pid[6] = pid_yaw_.Kp;
pid[7] = pid_yaw_.Ki;
pid[8] = pid_yaw_.Kd;
// xar PID
pid[9] = pid_xar_.Kp;
pid[10] = pid_xar_.Ki;
pid[11] = pid_xar_.Kd;
// yar PID
pid[12] = pid_yar_.Kp;
pid[13] = pid_yar_.Ki;
pid[14] = pid_yar_.Kd;
// zar PID
pid[15] = pid_zar_.Kp;
pid[16] = pid_zar_.Ki;
pid[17] = pid_zar_.Kd;
}
/* 获取力矩与升力接口函数*/
void GetForceApi(float *force)
{
force[0] = force_[0];
force[1] = force_[1];
force[2] = force_[2];
force[3] = force_[3];
}
— 完 —