一. 绪论

这一部分是核心内容，讲解姿态角的串级PID控制。在智能小车、四旋翼、四足狗子等等一系列机器人的控制系统中，姿态控制（俯仰角、滚转角、偏航角）都是核心内容，它决定了小车开得直不直，飞机飞得稳不稳。虽然现在先进的、智能的控制算法有很多，如自适应控制、神经网络控制、模糊控制等在机器人控制系统的设计上有了很多应用，但是最常用的最好用的依然是PID控制器，搞通了PID控制器就能够应付绝大多数场合了。

本文续接上一篇STM32实现四驱小车（三）传感任务——姿态角解算。

二. 角度环串级PID原理

1. PID基本算法

PID控制器的原理图如图所示。
PID控制器是一种线性控制器，根据给定值和实际输出值的偏差构成控制偏差
e(t)=yd(t)−y(t)e(t)={{y}_{d}}(t)-y(t)e(t)=yd(t)−y(t)
PID的控制率为
u(t)=kp[e(t)+1TI∫0te(t)dt+TDde(t)dt]u(t)={{k}_{p}}\left[ e(t)+\frac{1}{{{T}_{I}}}\int_{0}^{t}{e(t)dt+{{T}_{D}}\frac{de(t)}{dt}} \right]u(t)=kp[e(t)+TI1∫0te(t)dt+TDdtde(t)]
其中，kpk_pkp为比例系数，TIT_ITI为积分时间常数，TDT_DTD为微分时间常数。PID控制器各校正环节的作用为：
（1）比例环节：成比例的反应控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用，以减少偏差。但是比例环节不能消除稳态误差。
（2）积分环节：主要是消除静差，提高系统的无差度。积分作用的强弱取决于积分时间常数TIT_ITI，TIT_ITI越大，积分作用越弱，反之则越强。
（3）微分环节：反映偏差信号的变化趋势（变化速率），并能在偏差信号变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减少调节时间。

如何调节PID参数是实现PID控制器的核心内容，以笔者的经验，比例环节是起主要调节作用的，从小到大逐渐调整，直到系统有发散的趋势，然后往回取一个适中的值；积分环节的作用是消除误差，确定了比例系数后，从小到大增大积分系数（减少积分时间常数），直到系统有发散的趋势，积分环节不需要取得很大，记住它的作用是消除误差。微分环节的作用是超前校正，但是在噪声较大的情况下会放大噪声，引起系统不稳定，所以对于延迟没有太高要求的场合可以不加微分环节。

在实际中我们都是用的离散系统，所以我们关心数字PID控制的实现。在应用中一般有位置式PID控制和增量式PID控制 。

位置式PID的算法为：
u(k)=kpe(k)+ki∑j=0ke(j)T+kde(k)−e(k−1)Tu(k)={{k}_{p}}e(k)+{{k}_{i}}\sum\limits_{j=0}^{k}{e(j)}T+{{k}_{d}}\frac{e(k)-e(k-1)}{T}u(k)=kpe(k)+kij=0∑ke(j)T+kdTe(k)−e(k−1)
式中，T为采样周期，也就是单片机的控制周期。k为采样序列，e(k)和e(k-1)分别是第k次和第k-1次所得的偏差信号。

当执行机构需要的是控制量的增量时（例如驱动步进电机），应该采用增强式PID控制。由位置式PID的算法：
u(k−1)=kpe(k−1)+ki∑j=0k−1e(j)T+kde(k−1)−e(k−2)Tu(k-1)={{k}_{p}}e(k-1)+{{k}_{i}}\sum\limits_{j=0}^{k-1}{e(j)}T+{{k}_{d}}\frac{e(k-1)-e(k-2)}{T}u(k−1)=kpe(k−1)+kij=0∑k−1e(j)T+kdTe(k−1)−e(k−2)
得到增量式PID算法为：
Δu(k)=u(k)−u(k−1)=kp[e(k)−e(k−1)]+kie(k)+kd[e(k)−2e(k−1)+e(k−2)]\Delta u(k)=u(k)-u(k-1)=k_{p}[e(k)-e(k-1)]+k_{i} e(k)+k_{d}[e(k)-2 e(k-1)+e(k-2)] Δu(k)=u(k)−u(k−1)=kp[e(k)−e(k−1)]+kie(k)+kd[e(k)−2e(k−1)+e(k−2)]

2. 姿态角串级PID原理

对于姿态角的控制，我们希望给定姿态角机器人能够跟随给定的输入，其实这就是一个位置跟踪问题。按照单级PID的思路应该是这样的：
但是这里不用这种方式，而是采用串级PID，也就是一个PID套一个PID，外面是角度环，里面是角速度环。这样做的好处是增加了控制系统的响应速度和稳态精度，具体的原理大家可以去找文章专门研究，这里不过多讲解。

三. 如何用STM32实现角度-角速度的串级PID控制

1. PID算法的代码实现

原理弄明白之后其实实现起来很简单，PID的控制算法是通用的，完全可以移植，只要调整三个系数以适应自己做的东西就可以了，这里我们一起写一下，建立一个pid.h和一个pid.c文件，添加到工程中。
pid.h的内容如下，定义PID的结构体和一些数据结构、声明函数。

#ifndef __PID_H
#define __PID_H
#include "sys.h"
#include "stdbool.h"typedef struct
{float kp;float ki;float kd;
} pidInit_t;typedef struct
{pidInit_t roll;pidInit_t pitch;pidInit_t yaw;
} pidParam_t;typedef struct
{pidInit_t vx;pidInit_t vy;pidInit_t vz;
} pidParamPos_t;typedef struct
{pidParam_t pidAngle;  /*角度PID*/pidParam_t pidRate;   /*角速度PID*/pidParamPos_t pidPos; /*位置PID*/float thrustBase;          /*油门基础值*/u8 cksum;
} configParam_t;typedef struct
{float desired;  //< set pointfloat error;    //< errorfloat prevError; //< previous errorfloat integ;  //< integralfloat deriv;     //< derivativefloat kp;      //< proportional gainfloat ki;       //< integral gainfloat kd;       //< derivative gainfloat outP;       //< proportional output (debugging)float outI;       //< integral output (debugging)float outD;       //< derivative output (debugging)float iLimit;   //< integral limitfloat iLimitLow; //< integral limitfloat maxOutput;float dt; //< delta-time dt
} PidObject;/*pid结构体初始化*/
void pidInit(PidObject *pid, const float desired, const pidInit_t pidParam, const float dt);
void pidParaInit(PidObject *pid, float maxOutput, float iLimit, const pidInit_t pidParam);
void pidSetIntegralLimit(PidObject *pid, const float limit); /*pid积分限幅设置*/
void pidSetOutLimit(PidObject *pid, const float maxoutput);  /*pid输出限幅设置*/
void pidSetDesired(PidObject *pid, const float desired);     /*pid设置期望值*/
float pidUpdate(PidObject *pid, const float error);          /*pid更新*/
float pidGetDesired(PidObject *pid);                         /*pid获取期望值*/
bool pidIsActive(PidObject *pid);                            /*pid状态*/
void pidReset(PidObject *pid);                               /*pid结构体复位*/
void pidSetError(PidObject *pid, const float error);         /*pid偏差设置*/
void pidSetKp(PidObject *pid, const float kp);               /*pid Kp设置*/
void pidSetKi(PidObject *pid, const float ki);               /*pid Ki设置*/
void pidSetKd(PidObject *pid, const float kd);               /*pid Kd设置*/
void pidSetPID(PidObject *pid, const float kp, const float ki, const float kd);
void pidSetDt(PidObject *pid, const float dt); /*pid dt设置*/#endif /* __PID_H */

pid.c当中实现函数：

#include <stdbool.h>
#include "pid.h"void abs_outlimit(float *a, float ABS_MAX){if(*a > ABS_MAX)*a = ABS_MAX;if(*a < -ABS_MAX)*a = -ABS_MAX;
}void pidInit(PidObject* pid, const float desired, const pidInit_t pidParam, const float dt)
{pid->error     = 0;pid->prevError = 0;pid->integ     = 0;pid->deriv     = 0;pid->desired = desired;pid->kp = pidParam.kp;pid->ki = pidParam.ki;pid->kd = pidParam.kd;pid->iLimit    = DEFAULT_PID_INTEGRATION_LIMIT;pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT;pid->dt        = dt;
}float pidUpdate(PidObject* pid, const float error)
{float output;pid->error = error;   pid->integ += pid->error * pid->dt;pid->deriv = (pid->error - pid->prevError) / pid->dt;pid->outP = pid->kp * pid->error;pid->outI = pid->ki * pid->integ;pid->outD = pid->kd * pid->deriv;abs_outlimit(&(pid->integ), pid->iLimit);output = pid->outP + pid->outI + pid->outD;abs_outlimit(&(output), pid->maxOutput);pid->prevError = pid->error;return output;
}void pidSetIntegralLimit(PidObject* pid, const float limit)
{pid->iLimit = limit;
}void pidSetIntegralLimitLow(PidObject* pid, const float limitLow)
{pid->iLimitLow = limitLow;
}void pidSetOutLimit(PidObject* pid, const float maxoutput)
{pid->maxOutput = maxoutput;
}void pidReset(PidObject* pid)
{pid->error     = 0;pid->prevError = 0;pid->integ     = 0;pid->deriv     = 0;
}void pidSetError(PidObject* pid, const float error)
{pid->error = error;
}void pidSetDesired(PidObject* pid, const float desired)
{pid->desired = desired;
}float pidGetDesired(PidObject* pid)
{return pid->desired;
}bool pidIsActive(PidObject* pid)
{bool isActive = true;if (pid->kp < 0.0001f && pid->ki < 0.0001f && pid->kd < 0.0001f){isActive = false;}return isActive;
}void pidSetKp(PidObject* pid, const float kp)
{pid->kp = kp;
}void pidSetKi(PidObject* pid, const float ki)
{pid->ki = ki;
}void pidSetKd(PidObject* pid, const float kd)
{pid->kd = kd;
}void pidSetPID(PidObject* pid, const float kp,const float ki,const float kd)
{pid->kp = kp;pid->ki = ki;pid->kd = kd;
}
void pidSetDt(PidObject* pid, const float dt)
{pid->dt = dt;
}

这一部分代码大家自行阅读，很好理解，另外大家如果嫌函数太多可以用C++来用对象实现PID结构体。（网上有，不想自己写去copy也行）

2. 串级PID算法的代码实现

由于我们要使用串级PID控制航向角，仅仅有上面的PID控制器代码还不够，咱们继续创建一个attitude_control.h和一个attitude_control.c文件，用来实现串级PID控制。

attitude_control.h文件内容如下：

#ifndef __ATTITUDE_PID_H
#define __ATTITUDE_PID_H
#include <stdbool.h>
#include "pid.h"#define ATTITUDE_UPDATE_RATE  500  //更新频率100hz
#define ATTITUDE_UPDATE_DT      (1.0f / ATTITUDE_UPDATE_RATE)typedef struct
{float x;float y;float z;
} Axis3f;//姿态集
typedef struct
{float roll;float pitch;float yaw;
} attitude_t;extern PidObject pidAngleRoll;
extern PidObject pidAnglePitch;
extern PidObject pidAngleYaw;
extern PidObject pidRateRoll;
extern PidObject pidRatePitch;
extern PidObject pidRateYaw;
extern PidObject pidDepth;
extern configParam_t configParamCar;void attitudeControlInit(void);
bool attitudeControlTest(void);void attitudeRatePID(attitude_t *actualRate, attitude_t *desiredRate,attitude_t *output);    /* 角速度环PID */
void attitudeAnglePID(attitude_t *actualAngle,attitude_t *desiredAngle,attitude_t *outDesiredRate); /* 角度环PID */
void attitudeResetAllPID(void);     /*复位PID*/
void attitudePIDwriteToConfigParam(void);#endif /* __ATTITUDE_PID_H */

attitude_control.c文件内容如下：

#include <stdbool.h>
#include "pid.h"
#include "sensor.h"
#include "attitude_pid.h"//pid参数
configParam_t configParamCar =
{.pidAngle=    /*角度PID*/{  .roll={.kp=5.0,.ki=0.0,.kd=0.0,},.pitch={.kp=5.0,.ki=0.0,.kd=0.0,},.yaw={.kp=5.0,.ki=0.0,.kd=0.0,},},   .pidRate=  /*角速度PID*/{ .roll={.kp=320.0,.ki=0.0,.kd=5.0,},.pitch={.kp=320.0,.ki=0.0,.kd=5.0,},.yaw={.kp=18.0,.ki=0.2,.kd=0.0,},},  .pidPos=   /*位置PID*/{  .vx={.kp=0.0,.ki=0.0,.kd=0.0,},.vy={.kp=0.0,.ki=0.0,.kd=0.0,},.vz={.kp=21.0,.ki=0.0,.kd=60.0,},},};PidObject pidAngleRoll;
PidObject pidAnglePitch;
PidObject pidAngleYaw;
PidObject pidRateRoll;
PidObject pidRatePitch;
PidObject pidRateYaw;
PidObject pidDepth;static inline int16_t pidOutLimit(float in)
{if (in > INT16_MAX)return INT16_MAX;else if (in < -INT16_MAX)return -INT16_MAX;elsereturn (int16_t)in;
}void attitudeControlInit()
{//pidInit(&pidAngleRoll, 0, configParamCar.pidAngle.roll, ATTITUDE_UPDATE_DT);   /*roll  角度PID初始化*///pidInit(&pidAnglePitch, 0, configParamCar.pidAngle.pitch, ATTITUDE_UPDATE_DT); /*pitch 角度PID初始化*/pidInit(&pidAngleYaw, 0, configParamCar.pidAngle.yaw, ATTITUDE_UPDATE_DT);      /*yaw   角度PID初始化*///pidSetIntegralLimit(&pidAngleRoll, PID_ANGLE_ROLL_INTEGRATION_LIMIT);           /*roll  角度积分限幅设置*///pidSetIntegralLimit(&pidAnglePitch, PID_ANGLE_PITCH_INTEGRATION_LIMIT);         /*pitch 角度积分限幅设置*/pidSetIntegralLimit(&pidAngleYaw, PID_ANGLE_YAW_INTEGRATION_LIMIT);               /*yaw   角度积分限幅设置*/pidSetOutLimit(&pidAngleYaw, PID_ANGLE_YAW_INTEGRATION_LIMIT);//pidInit(&pidRateRoll, 0, configParamCar.pidRate.roll, ATTITUDE_UPDATE_DT);  /*roll  角速度PID初始化*///pidInit(&pidRatePitch, 0, configParamCar.pidRate.pitch, ATTITUDE_UPDATE_DT); /*pitch 角速度PID初始化*/pidInit(&pidRateYaw, 0, configParamCar.pidRate.yaw, ATTITUDE_UPDATE_DT);   /*yaw   角速度PID初始化*///pidSetIntegralLimit(&pidRateRoll, PID_RATE_ROLL_INTEGRATION_LIMIT);            /*roll  角速度积分限幅设置*///pidSetIntegralLimit(&pidRatePitch, PID_RATE_PITCH_INTEGRATION_LIMIT);      /*pitch 角速度积分限幅设置*/pidSetIntegralLimit(&pidRateYaw, PID_RATE_YAW_INTEGRATION_LIMIT);            /*yaw   角速度积分限幅设置*/pidSetOutLimit(&pidRateYaw, PID_RATE_YAW_INTEGRATION_LIMIT);
}void attitudeRatePID(attitude_t *actualRate, attitude_t *desiredRate, attitude_t *output) /* 角速度环PID */
{//output->roll = pidOutLimit(pidUpdate(&pidRateRoll, desiredRate->roll - actualRate->roll));//output->pitch = pidOutLimit(pidUpdate(&pidRatePitch, desiredRate->pitch - actualRate->pitch));output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, desiredRate->yaw - actualRate->yaw));
}void attitudeAnglePID(attitude_t *actualAngle, attitude_t *desiredAngle, attitude_t *outDesiredRate) /* 角度环PID */
{//outDesiredRate->roll = pidUpdate(&pidAngleRoll, desiredAngle->roll - actualAngle->roll);//outDesiredRate->pitch = pidUpdate(&pidAnglePitch, desiredAngle->pitch - actualAngle->pitch);float yawError = desiredAngle->yaw - actualAngle->yaw;if (yawError > 180.0f)yawError -= 360.0f;else if (yawError < -180.0)yawError += 360.0f;outDesiredRate->yaw = pidUpdate(&pidAngleYaw, yawError);
}void attitudeResetAllPID(void) /*复位PID*/
{pidReset(&pidAngleRoll);pidReset(&pidAnglePitch);pidReset(&pidAngleYaw);pidReset(&pidRateRoll);pidReset(&pidRatePitch);pidReset(&pidRateYaw);
}

attitude_control.c文件一开始声明并初始化了一个结构体变量configParamCar ，类型为configParam(在pid.h中定义的)，里面保存的就是小车所有PID的参数值，后续要做的就是对这个结构体进行PID调参。

大家可能注意到了attitudeControlInit(), attitudeRatePID(), attitudeAnglePID里面全部都有三轴的角度，只不过我屏蔽掉了俯仰角和滚装角，因为对于小车来说我们只需要航向角。后期实现四旋翼我们依然用的这一套代码框架，届时只需要使能其他两个角度就能实现四旋翼的姿态控制了。

四. UCOS-III姿态控制任务的实现

有了上面的驱动代码和PID算法，下面我们写main.c文件里面的StabilizationTask，实现姿态控制任务。

在上一篇STM32实现四驱小车（三）传感任务——姿态角解算的基础上，补充StabilizationTask函数的内容如下：

//stabilization姿态控制任务
void stabilization_task(void *p_arg)
{OS_ERR err;CPU_SR_ALLOC();int dt_ms = 1000 / ATTITUDE_UPDATE_RATE; //姿态数据采样周期,默认500Hz,2msfloat ft = (float)(dt_ms) / 1000.0;      //积分间隔，单位秒float throttle_base;                   //油门基础值，由油门通道决定float zoom_factor = 0.10f;               //转弯角速度attitude_t realAngle, expectedAngle, expectedRate;attitude_t realRate, output;attitudeControlInit();while (1){/********************************   航向角姿态控制  ****************************************/
/********************************   油门 控制      ****************************************///zoom_factor速度放大因子expectedAngle.yaw -= (float)(command[YAW]) * zoom_factor * ft;if (expectedAngle.yaw > 180.0f)expectedAngle.yaw -= 360.0f;if (expectedAngle.yaw < -180.0f)expectedAngle.yaw += 360.0f;//油门值,最高速9000，减速输出400rpmif (command[SPEED_MODE] == HIGH_SPEED)throttle_base = (float)(command[THROTTLE] * 8);else if (command[SPEED_MODE] == LOW_SPEED)throttle_base = (float)(command[THROTTLE] * 4);//没有油门输出，也没有转弯信号，此时机器人在静止状态//始终把当前姿态角作为期望姿态角//不使能PID计算，复位所有PIDif (command[THROTTLE] == 0 && command[YAW] == 0){expectedAngle.yaw = realAngle.yaw;attitudeResetAllPID(); //PID复位expectedRate.yaw = 0;output.yaw = 0;}//有油门输出，说明机器人在运动状态，此时应该做姿态控制else{//姿态角串级pid计算attitudeAnglePID(&realAngle, &expectedAngle, &expectedRate); /* 角度环PID */attitudeRatePID(&realRate, &expectedRate, &output);             /* 角速度环PID */}//pid控制量分配到电机混控set_speed[1] = throttle_base - output.yaw;set_speed[0] = set_speed[1];set_speed[3] = -(throttle_base + output.yaw);set_speed[2] = set_speed[3];//延时采样delay_ms(dt_ms);}
}

这里面while循环里面的步骤为，首先根据读到的遥控器的方向摇杆的值更新期望偏航角，期望偏航角来自于方向摇杆的积分。然后根据速度档位按钮的值确定当前的油门量（低速与高速模式）。之后判断遥控器油门摇杆与方向摇杆的位置，如果都居中说明机器人应该静止，此时复位所有PID，PID输出置零。如果任何一个摇杆不是中间位置，说明是在前进后退或者原地转弯状态，此时使能串级PID控制，控制器的输出送入到混合控制器（注意这个词，在飞控中还会用到），由于四驱车的模型很简单，其实就是一侧加上这个控制量加速，一侧减去这个控制量减速，从而实现差速，控制机器人转弯。

这里面有一个数组set_speed[4]，存储的是各个电机的速度，这个速度值在下一篇电机伺服任务中我们要用到，它作为期望速度值，作为电机速度伺服的PID控制器输入。

这里做下说明，本系列文章笔者重在分享思想、算法，在讲解上会弱化一些基本知识（比如单片机各个外设的原理、单片机编程的基本知识等），在代码的粘贴上会忽视一些底层的驱动代码和无关紧要的部分，事实上上面的代码我都经过删减了，只留下了干货。所以可以说面向的是中高级选手，拿来主义者可以打道回府了，本系列文章不开源，不提供源码，请见谅。

STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法相关推荐

基于串级 PID 控制算法的四旋翼无人机控制系统设计与实现
1.内容简介略 489-可以交流.咨询.答疑 2.内容说明无人机,英文名称UAV C Unmanned Aerial Vehicle,是一种特殊的飞行器, 可以按照预先设定的轨迹进行各种运动, ...
STM32实现四驱小车（五）电机控制任务——电机速度PID控制算法
目录一. 绪论二. 电机速度环PID原理三. STM32使用CAN总线实现大疆M3508电机的速度闭环控制四. UCOS-III电机控制任务的实现一. 绪论本文接上一篇STM32实现四驱小 ...
STM32实现四驱小车（三）传感任务——姿态角解算
目录一. 绪论二. 惯性传感器测量原理 1. 三轴加速度计 2. 三轴陀螺仪 3. 三轴磁力计三. 状态估计 1. 姿态估计 (1)线性互补滤波器 (2)非线性互补滤波器 (3)卡尔曼滤波器 2 ...
STM32实现四驱小车（一）硬件与软件准备
目录一. 绪论二. 轮式机器人概述三. 硬件准备 1. 机械底盘 2. 电机选择 3. 驱动板 4. 传感器 5. 电池四. 软件准备--UCOS-III操作系统一. 绪论匆匆忙忙的202 ...
2021电赛F题智能送药小车方案分析(openMV数字识别,红线循迹,STM32HAL库freeRTOS,串级PID快速学习,小车自动返回）
2021全国大学生电子设计竞赛F题智能送药小车前提:本篇文章重在分享自己的心得与感悟,我们把最重要的部分,摄像头循迹,摄像头数字识别问题都解决了,有两种方案一种是openARTmini摄像头进行数字 ...
【平衡小车制作】（七）串级PID调参及平衡成果展示（超详解）
大家好,我是小政.本篇文章我将针对PID调参进行详细的讲解,让每位小伙伴能够对比例.积分.微分三个参数如何调节有更加清晰的理解. 一.调参步骤确立机械中值直立环(内环)--Kp极性.Kp大小. ...
学习四旋翼（三）：DMP姿态解算和串级PID控制姿态
暑假期间,对于四旋翼有一点兴趣,没有亲手做,但是看了一些资料.这个系列文章只是对自己看的东西的记录,对于想要学习了解相关知识的同学没有任何参考价值! 本篇是系列第三篇,介绍了我对于MPU9250 DM ...
为什么需要串级PID控制（结合智能小车，四轴飞行器来解释）
先说四轴飞行器四轴飞行器中串级PID控制是由角度环与角速度环一起控制的可以这么简单的理解: 角度环可以可以保证飞机按期望的角度飞行,单环控制时,具有很好的自稳性.但是打舵的时候跟随性就不够完美.可 ...
基于串级PID的平衡小车
一般的平衡小车是以速度环为外环,外环输出作为内环角度输入的反馈,与期望角度叠加,通过角度的浮动控制实现速度闭环,本次平衡小车结合了四轴无人机的串级PID 方法,引入角速度环,方便参数调节.主控采用st ...

STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法

目录