目錄
- 一. 緒論
- 二. 角度環串級PID原理
-
- 1. PID基本算法
- 2. 姿态角串級PID原理
- 三. 如何用STM32實作角度-角速度的串級PID控制
-
- 1. PID算法的代碼實作
- 2. 串級PID算法的代碼實作
- 四. UCOS-III姿态控制任務的實作
一. 緒論
這一部分是核心内容，講解姿态角的串級PID控制。在智能小車、四旋翼、四足狗子等等一系列機器人的控制系統中，姿态控制（俯仰角、滾轉角、偏航角）都是核心内容，它決定了小車開得直不直，飛機飛得穩不穩。雖然現在先進的、智能的控制算法有很多，如自适應控制、神經網絡控制、模糊控制等在機器人控制系統的設計上有了很多應用，但是最常用的最好用的依然是PID控制器，搞通了PID控制器就能夠應付絕大多數場合了。
本文續接上一篇STM32實作四驅小車（三）傳感任務——姿态角解算。
二. 角度環串級PID原理
1. PID基本算法
PID控制器的原理圖如圖所示。
PID控制器是一種線性控制器，根據給定值和實際輸出值的偏差構成控制偏差
e ( t ) = y d ( t ) − y ( t ) e(t)={{y}_{d}}(t)-y(t) e(t)=yd(t)−y(t)
PID的控制率為
u ( t ) = k p [ e ( t ) + 1 T I ∫ 0 t e ( t ) d t + T D d e ( t ) d t ] 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)]
其中， k p k_p kp為比例系數， T I T_I TI為積分時間常數， T D T_D TD為微分時間常數。PID控制器各校正環節的作用為：
（1）比例環節：成比例的反應控制系統的偏差信号e(t)，偏差一旦産生，控制器立即産生控制作用，以減少偏差。但是比例環節不能消除穩态誤差。
（2）積分環節：主要是消除靜差，提高系統的無差度。積分作用的強弱取決于積分時間常數 T I T_I TI， T I T_I TI越大，積分作用越弱，反之則越強。
（3）微分環節：反映偏差信号的變化趨勢（變化速率），并能在偏差信号變得太大之前，在系統中引入一個有效的早期修正信号，進而加快系統的動作速度，減少調節時間。
如何調節PID參數是實作PID控制器的核心内容，以筆者的經驗，比例環節是起主要調節作用的，從小到大逐漸調整，直到系統有發散的趨勢，然後往回取一個适中的值；積分環節的作用是消除誤差，确定了比例系數後，從小到大增大積分系數（減少積分時間常數），直到系統有發散的趨勢，積分環節不需要取得很大，記住它的作用是消除誤差。微分環節的作用是超前校正，但是在噪聲較大的情況下會放大噪聲，引起系統不穩定，是以對于延遲沒有太高要求的場合可以不加微分環節。
在實際中我們都是用的離散系統，是以我們關心數字PID控制的實作。在應用中一般有位置式PID控制和增量式PID控制 。
位置式PID的算法為：
u ( k ) = k p e ( k ) + k i ∑ j = 0 k e ( j ) T + k d e ( k ) − e ( k − 1 ) T 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} 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 ) = k p e ( k − 1 ) + k i ∑ j = 0 k − 1 e ( j ) T + k d e ( k − 1 ) − e ( k − 2 ) T 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} 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 ) = k p [ e ( k ) − e ( k − 1 ) ] + k i e ( k ) + k d [ e ( k ) − 2 e ( 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 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控制器輸入。
這裡做下說明，本系列文章筆者重在分享思想、算法，在講解上會弱化一些基本知識（比如單片機各個外設的原理、單片機程式設計的基本知識等），在代碼的粘貼上會忽視一些底層的驅動代碼和無關緊要的部分，事實上上面的代碼我都經過删減了，隻留下了幹貨。是以可以說面向的是中進階選手，拿來主義者可以打道回府了，本系列文章不開源，不提供源碼，請見諒。