STM32实现水下四旋翼（九）飞行控制任务1——姿态角串级PID控制

最新推荐文章于 2023-09-19 10:35:41 发布

何为其然

最新推荐文章于 2023-09-19 10:35:41 发布

阅读量3.5k

点赞数 12

分类专栏：嵌入式 STM32 文章标签： stm32 嵌入式算法

本文链接：https://blog.csdn.net/qq_30267617/article/details/114584478

版权

嵌入式同时被 2 个专栏收录

32 篇文章 106 订阅

订阅专栏

STM32

26 篇文章 35 订阅

订阅专栏

一. 绪论

从这一篇开始就是姿态控制的内容了，这是核心算法部分。经过前面的部分我们实现了遥控器通信和传感器数据解析，解决了输入与反馈的问题。在闭环控制系统中，核心就是控制器的设计了。在智能小车、四旋翼、四足狗子等等一系列机器人的控制系统中，姿态控制（俯仰角、滚转角、偏航角）都是核心内容，它决定了小车开得直不直，飞机飞得稳不稳。虽然现在先进的、智能的控制算法有很多，如自适应控制、神经网络控制、模糊控制等在机器人控制系统的设计上有了很多应用，但是最常用的最好用的依然是PID控制器，搞通了PID控制器就能够应付绝大多数场合了。

二. 角度环串级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\limits_{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\limits_{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)-2 e(k-1)+e(k-2)]$

2. 串级PID控制原理

对于姿态角的控制，我们希望给定姿态角机器人能够跟随给定的输入，其实这就是一个位置跟踪问题。按照单级PID的思路应该是这样的：
在这里插入图片描述但是这里不用这种方式，而是采用串级PID，也就是一个PID套一个PID，外面是角度环，里面是角速度环。这样做的好处是增加了控制系统的响应速度和稳态精度，具体的原理大家可以去找文章专门研究，这里不过多讲解。
在这里插入图片描述以上表示的是一个角度的串级PID控制，现在我们有三个角度，滚转角、俯仰角和航向角，那么就需要做三个串级PID控制器，很幸运的是算法是一样的，我们只需要调整参数就行了。

三. 角度-角速度串级PID控制实现

1. PID控制器的实现

首先我们需要实现PID计算的功能，按照PID的算法，很容易实现。在之前的STM32实现水下四旋翼（五）自定义航行数据中我们创建过pid.c和pid.h文件，那个时候我们只在里面定义了一些结构体和结构体变量，用来存储PID参数，方便后面调参用。现在我们在里面增加PID控制的数据定义和函数。

pid.h文件里面添加

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;

void pidInit(PidObject *pid, const float desired, const pidInit_t pidParam, const float dt);
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设置*/

pid.c中增加内容，定义函数，相应函数的功能由名字就可以看出，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;
}

2. 串级PID控制器的实现

好的，现在我们有PID计算的代码了。下面继续实现串级pid的函数。创建attitude_pid.c和attitude_pid.h.

attitude_pid.h中添加下面内容：

#ifndef __ATTITUDE_PID_H
#define __ATTITUDE_PID_H
#include <stdbool.h>
#include "pid.h"
#include "stabilizer.h"

#define ATTITUDE_UPDATE_RATE 	200  //更新频率100hz
#define ATTITUDE_UPDATE_DT 		(1.0f / ATTITUDE_UPDATE_RATE)

/*角度环积分限幅*/
#define PID_ANGLE_ROLL_INTEGRATION_LIMIT 3000.0
#define PID_ANGLE_PITCH_INTEGRATION_LIMIT 3000.0
#define PID_ANGLE_YAW_INTEGRATION_LIMIT 3000.0

/*角速度环积分限幅*/
#define PID_RATE_ROLL_INTEGRATION_LIMIT 10000.0
#define PID_RATE_PITCH_INTEGRATION_LIMIT 10000.0
#define PID_RATE_YAW_INTEGRATION_LIMIT 10000.0

extern PidObject pidAngleRoll;
extern PidObject pidAnglePitch;
extern PidObject pidAngleYaw;
extern PidObject pidRateRoll;
extern PidObject pidRatePitch;
extern PidObject pidRateYaw;

void attitudeControlInit(void);
void attitudeRatePID(attitude_t *actualRate, attitude_t *desiredRate,control_t *output);	/* 角速度环PID */
void attitudeAnglePID(attitude_t *actualAngle,attitude_t *desiredAngle,attitude_t *outDesiredRate);	/* 角度环PID */
void attitudeResetAllPID(void);		/*复位PID*/
#endif /* __ATTITUDE_PID_H */

attitude_pid.c中添加下面内容：

#include <stdbool.h>
#include "pid.h"
#include "sensor.h"
#include "attitude_pid.h"
#include "stabilizer.h"

PidObject pidAngleRoll;
PidObject pidAnglePitch;
PidObject pidAngleYaw;
PidObject pidRateRoll;
PidObject pidRatePitch;
PidObject pidRateYaw;


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, configParam.pidAngle.roll, ATTITUDE_UPDATE_DT);   /*roll  角度PID初始化*/
	pidInit(&pidAnglePitch, 0, configParam.pidAngle.pitch, ATTITUDE_UPDATE_DT); /*pitch 角度PID初始化*/
	pidInit(&pidAngleYaw, 0, configParam.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(&pidAngleRoll, PID_ANGLE_ROLL_INTEGRATION_LIMIT);
	pidSetOutLimit(&pidAnglePitch, PID_ANGLE_PITCH_INTEGRATION_LIMIT);
	pidSetOutLimit(&pidAngleYaw, PID_ANGLE_YAW_INTEGRATION_LIMIT);

	pidInit(&pidRateRoll, 0, configParam.pidRate.roll, ATTITUDE_UPDATE_DT);	 /*roll  角速度PID初始化*/
	pidInit(&pidRatePitch, 0, configParam.pidRate.pitch, ATTITUDE_UPDATE_DT); /*pitch 角速度PID初始化*/
	pidInit(&pidRateYaw, 0, configParam.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(&pidRateRoll, PID_RATE_ROLL_INTEGRATION_LIMIT);
	pidSetOutLimit(&pidRatePitch, PID_RATE_PITCH_INTEGRATION_LIMIT);
	pidSetOutLimit(&pidRateYaw, PID_RATE_YAW_INTEGRATION_LIMIT);

}

void attitudeRatePID(attitude_t *actualRate, attitude_t *desiredRate, control_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);
}

这里面主要实现四个函数。

void attitudeControlInit()用于PID参数的初始化，

void attitudeAnglePID(attitude_t *actualAngle, attitude_t *desiredAngle, attitude_t *outDesiredRate)实现角度环PID计算。

void attitudeRatePID(attitude_t *actualRate, attitude_t *desiredRate, control_t *output)实现角速度环计算。

void attitudeResetAllPID(void)复位所有PID的计算数据，一般在模式切换、油门归零等场合调用。

四. 姿态控制任务创建与实现

1. 姿态控制任务创建

好的，经过上面两组文件（pid.c和pid.h，attitude_pid.c和attitude_pid.h），我们有了计算的工具了。下面就要实现激动人心的姿态控制任务了。来到我们的main文件，前面我们创建了遥控器通信任务和传感任务，现在我们创建一个姿态控制任务，在文件起始部位接前面两个任务添加任务堆栈、任务函数等内容声明：

//stabalizer任务
//设置任务优先级
#define STABILIZATION_TASK_PRIO 4
//任务堆栈大小
#define STABILIZATION_STK_SIZE 2048
//任务控制块
OS_TCB StabilizationTaskTCB;
//任务堆栈
CPU_STK STABILIZATION_TASK_STK[STABILIZATION_STK_SIZE];
//led0任务
void stabilization_task(void *p_arg);

在void start_task(void *p_arg)中接前面两个任务添加姿态控制任务的创建函数：

OSTaskCreate((OS_TCB *)&StabilizationTaskTCB,
				 (CPU_CHAR *)"Stabilization task",
				 (OS_TASK_PTR)stabilization_task,
				 (void *)0,
				 (OS_PRIO)STABILIZATION_TASK_PRIO,
				 (CPU_STK *)&STABILIZATION_TASK_STK[0],
				 (CPU_STK_SIZE)STABILIZATION_STK_SIZE / 10,
				 (CPU_STK_SIZE)STABILIZATION_STK_SIZE,
				 (OS_MSG_QTY)0,
				 (OS_TICK)10,
				 (void *)0,
				 (OS_OPT)OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR |OS_OPT_TASK_SAVE_FP,
				 (OS_ERR *)&err);

2. 姿态控制任务应用程序

姿态控制任务太重要了，它的功能为：

输入数据：输入数据包括遥控器通讯任务中读到的遥控器各个通道值，还有传感任务中读到的各个传感器的数据。通道值经过转换后变为控制指令，传感数据是反馈数据，现在期望数据（来自遥控器）有了，反馈数据（来自传感器）有了。
串级PID计算：将期望数据与实际数据进行比较，进行串级PID计算。
输出数据：串级PID计算将会输出结果（三组角度环输出三个数据），同时还会输出油门值等数据。这些数据经过控制分配最后会作用到电机。

好的，按照这个步骤，我们补充姿态控制任务：

void stabilization_task(void *p_arg)
{
	OS_ERR err;
	CPU_SR_ALLOC();
	
	u8 lenth = sizeof(configParam);
	lenth = lenth / 4 + (lenth % 4 ? 1 : 0);
	STMFLASH_Read(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth);   //载入PID参数

	attitudeControlInit();
	
	while(1)
	{
	    Water_Attitude_Control(&control);
		delay_ms(5);
	}
}

在姿态控制任务中首先是从内存中读取PID参数值（这一部分内容后面会分一章讲），然后进行串级PID的初始化。之后进入while(1)循环，以5ms的控制周期进行控制，这里面封装了一个函数，其函数原型为
void Water_Attitude_Control(control_t *output);，功能就是水下姿态控制。定义如下：

void Water_Attitude_Control(control_t *output)
{
    float turn_speed;
    float z_speed;
    float forward_speed;

    turn_speed = pwm2Range(command[YAW], -1000.0f, 1000.0f);
    if (turn_speed < 30.0f && turn_speed > -30.0f)
        turn_speed = 0.f; // 死区
    z_speed = pwm2Range(command[THROTTLE], -1000.0f, 1000.0f);
    if (z_speed < 30.0f && z_speed > -30.0f)
        z_speed = 0.f; // 死区
 
/******************************************* 手动模式下控制 ********************************************/
    if (command[CARRY_MODE] == HAND_MODE)
    {
        // 手动模式下 油门通道为0时不运动，复位所有 姿态pid与pid输出
        control.thrust = pwm2thrust(command[THROTTLE]);
        setstate.expectedDepth = state.realDepth;          // 手动模式下期望高度始终等于当前高度/
                                                           // 切换到定高时从当前高度开始定高
        if (control.thrust < 200 && control.thrust > -200) 
            control.thrust = 0;                            // 油门死区

        if ((int)control.thrust == 0 && (int)turn_speed == 0) // 油门和方向摇杆都居中，机器人不使能
        {
            attitudeResetAllPID(); //PID复位
            setstate.expectedAngle.yaw = state.realAngle.yaw;
            setstate.expectedAngle.roll = state.realAngle.roll;
            setstate.expectedAngle.pitch = state.realAngle.pitch;
            control.yaw = 0;
            control.roll = 0;
            control.pitch = 0;
        }
        else // 油门有输出，从遥控器获得期望值，姿态PID		// 或者油门没输出，原地转圈
        {
            setstate.expectedAngle.roll = pwm2Range(command[ROLL], -30.0f, 30.0f);
            if (setstate.expectedAngle.roll < 0.9f && setstate.expectedAngle.roll > -0.9f)
                setstate.expectedAngle.roll = 0.f; // 摇杆死区
            setstate.expectedAngle.pitch = pwm2Range(command[PITCH], -30.0f, 30.0f);
            if (setstate.expectedAngle.pitch < 0.9f && setstate.expectedAngle.pitch > -0.9f)
                setstate.expectedAngle.pitch = 0.f; //遥感死区	
            setstate.expectedAngle.yaw -= turn_speed * zoom_factor_yaw * ft;
            if (setstate.expectedAngle.yaw > 180.0f)
                setstate.expectedAngle.yaw -= 360.0f;
            if (setstate.expectedAngle.yaw < -180.0f)
                setstate.expectedAngle.yaw += 360.0f;
            attitudeAnglePID(&state.realAngle, &setstate.expectedAngle, &setstate.expectedRate); /* 角度环PID */
            attitudeRatePID(&state.realRate, &setstate.expectedRate, &control);                  /* 角速度环PID */
        }
    }
/******************************************* 手动模式下控制 ********************************************/
}

void Water_Attitude_Control(control_t *output)暂时只实现了手动模式的控制，后面再讲解定深模式。上面代码中command打头的量都是遥控器通讯章节讲过的，表示将遥控器通道值转换为了相应的数据和命令，不记得可以回去看看。

经过串级PID的计算，我们得到了三个控制量，control.yaw，control.roll，control.pitch，油门量control.thrust已经通过遥控器给定了，那么如何运动这些控制量去控制四个电机的速度呢？这就是控制分配的问题了。下一讲讲解。

何为其然

关注

12
点赞
踩
84

收藏

觉得还不错? 一键收藏
打赏
6
评论
STM32实现水下四旋翼（九）飞行控制任务1——姿态角串级PID控制

目录一. 绪论二. 角度环串级PID原理1. PID控制原理2. 串级PID控制原理三. 角度-角速度串级PID控制实现1. PID控制器的实现2. 串级PID控制器的实现四. 姿态控制任务创建与实现1. 姿态控制任务创建2. 姿态控制任务应用程序一. 绪论从这一篇开始就是姿态控制的内容了，这是核心算法部分。经过前面的部分我们实现了遥控器通信和传感器数据解析，解决了输入与反馈的问题。在闭环控制系统中，核心就是控制器的设计了。在智能小车、四旋翼、四足狗子等等一系列机器人的控制系统中，姿态控制（俯仰角、滚转
复制链接

扫一扫