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

一. 绪论

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

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

二. 角度环串级PID原理

1. PID基本算法

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

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

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

位置式PID的算法为：
$$u(k)=k_{p}e(k)+k_i\sum _{j=0}^{k}e(j)T+k_d\frac{e(k)-e(k-1)}{T}$$
式中，T为采样周期，也就是单片机的控制周期。k为采样序列，e(k)和e(k-1)分别是第k次和第k-1次所得的偏差信号。

当执行机构需要的是控制量的增量时（例如驱动步进电机），应该采用增强式PID控制。由位置式PID的算法：
$$u(k-1)=k_{p}e(k-1)+k_i\sum _{j=0}^{k-1}e(j)T+k_d\frac{e(k-1)-e(k-2)}{T}$$
得到增量式PID算法为：
$$\Delta u(k)=u(k)-u(k-1)=k_p[e(k)-e(k-1)]+k_{i}e(k)+k_d[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 point
	float error;	 //< error
	float prevError; //< previous error
	float integ;	 //< integral
	float deriv;	 //< derivative
	float kp;		 //< proportional gain
	float ki;		 //< integral gain
	float kd;		 //< derivative gain
	float outP;		 //< proportional output (debugging)
	float outI;		 //< integral output (debugging)
	float outD;		 //< derivative output (debugging)
	float iLimit;	 //< integral limit
	float iLimitLow; //< integral limit
	float 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;
	else
		return (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,2ms
	float 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，减速输出400rpm
		if (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计算，复位所有PID
		if (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控制器输入。

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