356 lines
11 KiB
C
356 lines
11 KiB
C
/*
|
||
滑膜面调参目标使滑模面s趋近于0，趋近率选择：
|
||
指数趋近率
|
||
幂次趋近率
|
||
|
||
J给个固定的值，乘一个输出，J的大小取决于控制量受噪声影响
|
||
IMU噪声对其影响很大，J应该给大些，并且K和s不能给太大，如果基于编码器控制J建议直接给1
|
||
ε(epsilon)不宜过大，过大会影响精度,limit和ε给的值决定线性区间
|
||
K是增益，K大了也会抖,并且影响到目标位置是否能停下来，K太大会停不下来
|
||
c决定s趋近0的快满程度，c大了会抖
|
||
|
||
注意，滑膜控制是位置控制，这个位置变化是在控制频率内的，我的速度也要乘这个时间如（0.002）
|
||
否则速度误差占比会非常大，电机停不下来
|
||
输出u
|
||
|
||
参考资料
|
||
https://zhuanlan.zhihu.com/p/78549442
|
||
|
||
*/
|
||
#include "Sliding.h"
|
||
#include "math.h"
|
||
#include "user_math.h"
|
||
|
||
/**
|
||
* @brief 参数调整时使其连续
|
||
*
|
||
* @param Sliding s结构体
|
||
*/
|
||
static void OutContinuation(Sliding *s){
|
||
|
||
if(s->param->K != 0 && s->param->c2 != 0)
|
||
{
|
||
s->position.error_integral = (s->param_last->K / s->param->K) * (s->param_last->c2 / s->param->c2) * s->position.error_integral;
|
||
s->velocity.error_integral = (s->param_last->K / s->param->K) * (s->param_last->c2 / s->param->c2) * s->velocity.error_integral;
|
||
}
|
||
s->param_last = s->param;
|
||
}
|
||
|
||
/**
|
||
* @brief 符号函数
|
||
*
|
||
* @param s 滑膜面
|
||
* @return
|
||
*/
|
||
static float Signal(float s){
|
||
if (s > 0)
|
||
return 1;
|
||
else if (s == 0)
|
||
return 0;
|
||
else
|
||
return -1;
|
||
}
|
||
|
||
/**
|
||
* @brief 饱和函数
|
||
*滑模控制中用于代替符号函数的连续化近似函数，用于抑制滑模控制的高频抖振现象。
|
||
函数特性：
|
||
在滑模面 s 较小时（边界层内），输出与输入呈线性关系
|
||
在滑模面 s 较大时（边界层外），输出饱和为 ±1
|
||
连续可导（在边界点一阶连续），减少控制系统的高频切换
|
||
* @param s 滑膜面
|
||
* @param sliding 滑膜结构体
|
||
* @return Signal(y)/y
|
||
*/
|
||
static float Sat(float s,Sliding *sliding){
|
||
float y;
|
||
y = s / sliding->param->epsilon;
|
||
if (fabs(y) <= sliding->param->limit)
|
||
return y;
|
||
else
|
||
return Signal(y);
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 滑膜设立参数
|
||
*
|
||
* @param Sliding s结构体
|
||
* @param mode 滑膜模式
|
||
* @param param 滑膜参数
|
||
* @return
|
||
*/
|
||
static void SMC_SetParam(Sliding *s,Smc_mode mode,SlidingParam *param) {
|
||
///EXPONENT,POWER,VELSMC 参数设定
|
||
s->mode = mode;
|
||
switch(mode){
|
||
case EXPONENT:
|
||
case POWER:
|
||
case VELSMC:
|
||
s->param = param;
|
||
OutContinuation(s);
|
||
break;
|
||
|
||
case TFSMC:
|
||
break;
|
||
case EISMC:
|
||
s->param = param;
|
||
|
||
OutContinuation(s);
|
||
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 滑膜重置
|
||
*
|
||
* @param Sliding s结构体
|
||
* @return
|
||
*/
|
||
void SMC_Reset(Sliding *s){
|
||
/* 位置速度清零 */
|
||
s->position.tar_now = 0;
|
||
s->position.tar_last = 0;
|
||
s->position.tar_differential = 0;
|
||
s->velocity.tar_now = 0;
|
||
s->velocity.tar_last = 0;
|
||
s->velocity.tar_differential = 0;
|
||
/* 误差清零 */
|
||
s->position.error = 0;
|
||
s->velocity.error = 0;
|
||
|
||
s->position.error_eps = 0;
|
||
s->velocity.error_eps = 0;
|
||
s->position.now_get = 0;
|
||
s->velocity.now_get = 0;
|
||
/* 一阶导清零 */
|
||
s->velocity.tar_differential_last=0;
|
||
s->position.tar_differential_second=0;
|
||
s->position.tar_differential_last=0;
|
||
/* 积分清零 */
|
||
s->position.error_integral=0;
|
||
s->velocity.error_integral = 0;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 滑膜初始化，设立参数
|
||
*
|
||
* @param Sliding s结构体
|
||
* @param mode 滑膜模式
|
||
* @param param 滑膜参数
|
||
* @return
|
||
*/
|
||
void SMC_Init(Sliding *s,Smc_mode mode,SlidingParam *param){
|
||
|
||
s->param->J = 0;
|
||
s->param->K = 0;
|
||
s->param->c = 0;
|
||
s->param->epsilon = 0;
|
||
s->param->u_max = 0;
|
||
s->param->limit = 0;
|
||
|
||
s->position.tar_now = 0;
|
||
s->position.tar_last = 0;
|
||
s->position.tar_differential = 0;
|
||
|
||
s->position.error = 0;
|
||
s->velocity.error = 0;
|
||
s->velocity.error_integral = 0;
|
||
s->velocity.error_integral_max = 0;
|
||
s->position.error_eps = 0;
|
||
s->velocity.error_eps = 0;
|
||
s->position.now_get = 0;
|
||
s->velocity.now_get = 0;
|
||
|
||
/* 设立参数 */
|
||
SMC_SetParam(s,mode,param);
|
||
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 位置环速度更新
|
||
*
|
||
* @param Sliding s结构体
|
||
* @param target 目标位置
|
||
* @param pos_now 现在位置
|
||
* @param vol_now 现在速度
|
||
* @param dt 采样频率
|
||
* @return
|
||
*/
|
||
void SMC_PErrorUpdate(Sliding *s,float target, float pos_now, float vol_now,float dt) {
|
||
s->position.tar_now = target;
|
||
/* 一阶导(速度) */
|
||
s->position.tar_differential = (float)((s->position.tar_now - s->position.tar_last)/dt);
|
||
/* 二阶导 */
|
||
s->position.tar_differential_second=(float)((s->position.tar_differential- s->position.tar_differential_last)/dt);
|
||
|
||
s->position.error = pos_now - target;
|
||
s->velocity.error = vol_now - s->position.tar_differential;
|
||
|
||
/* 将误差限制在 -PI 到 +PI 之间 */
|
||
if (s->position.error > M_PI) {
|
||
s->position.error -= M_2PI;
|
||
} else if (s->position.error < -M_PI) {
|
||
s->position.error += M_2PI;
|
||
}
|
||
|
||
/* 更新上一次的值 */
|
||
s->position.tar_last = s->position.tar_now;
|
||
/* 位置误差积分项 */
|
||
s->position.error_integral += (float)(s->position.error * dt);
|
||
/* 二阶导更新 */
|
||
s->position.tar_differential_last = s->position.tar_differential;
|
||
|
||
}
|
||
|
||
/**
|
||
* @brief 速度环误差更新
|
||
*
|
||
* @param Sliding s结构体
|
||
* @param target 目标位置
|
||
* @param vol_now 现在速度
|
||
* @param dt 采样频率
|
||
* @return
|
||
*/
|
||
void SMC_VErrorUpdate(Sliding *s,float target,float vol_now,float dt) {
|
||
|
||
s->velocity.tar_now = target;
|
||
s->velocity.tar_differential = (float)((s->velocity.tar_now - s->velocity.tar_last)/dt);
|
||
|
||
// smc.error.tar_differential_second = (float)((smc.error.tar_differential- smc.error.tar_differential_last)/SAMPLE_PERIOD); ///二阶导
|
||
|
||
s->velocity.error = vol_now - s->velocity.tar_now;
|
||
/* 速度误差积分项 */
|
||
s->velocity.error_integral += (float)(s->velocity.error * dt);
|
||
// if(std::fabs(smc.error.v_error_integral) > smc.error.v_error_integral_max) //积分限幅
|
||
// {
|
||
// smc.error.v_error_integral = smc.error.v_error_integral_max;
|
||
// }
|
||
s->velocity.tar_last = s->velocity.tar_now;
|
||
|
||
|
||
}
|
||
|
||
|
||
//void SMC_Integval_Reset()
|
||
//{
|
||
// smc.error.v_error_integral = 0;
|
||
// smc.error.p_error_integral = 0;
|
||
//}
|
||
|
||
/**
|
||
* @brief 滑膜计算
|
||
*
|
||
* @param Sliding s结构体
|
||
* @param mode 滑膜模式
|
||
* @return u 输出电流
|
||
*/
|
||
float Smc_Calc(Sliding *s){
|
||
/* 输出，滑模面 */
|
||
float u,fun;
|
||
/* tfsmc 位置 幂 (这个是移值的，放在这待开发)*/
|
||
static float pos_pow;
|
||
/* 力矩 */
|
||
s->fun=fun;
|
||
switch (s->mode) {
|
||
case EXPONENT:///线性滑模面，指数趋近率
|
||
|
||
if (fabs(s->position.error) - s->position.error_eps < 0){
|
||
s->position.error = 0;
|
||
return 0;
|
||
}
|
||
/* 建立滑模面 */
|
||
s->s = s->param->c * s->position.error + s->velocity.error;
|
||
/* 饱和函数消除抖动 */
|
||
fun = Sat(s->s,s);
|
||
// u = smc.param.J * ( (-smc.param.c * smc.error.v_error) - smc.param.K * smc.s - smc.param.epsilon * fun + smc.error.tar_differential_second); //控制器计算,指数趋近率
|
||
/* 控制器计算,指数趋近率 */
|
||
u = s->param->J * ( (-s->param->c * s->velocity.error) - s->param->K * s->s - s->param->epsilon * fun);
|
||
s->s1=-s->param->c * s->velocity.error;
|
||
s->s2=s->param->K * s->s;
|
||
s->s3=s->param->epsilon * fun;
|
||
s->s4=fun;
|
||
break;
|
||
case POWER:///线性滑模面，幂次趋近率
|
||
|
||
if (fabs(s->position.error) - s->position.error_eps < 0){
|
||
s->position.error = 0;
|
||
return 0;
|
||
}
|
||
|
||
s->s = s->param->c * s->position.error + s->velocity.error; //滑模面
|
||
fun = Sat(s->s,s);//饱和函数消除抖动
|
||
// u = smc.param.J * ( (-smc.param.c * smc.error.v_error) - smc.param.K * smc.s - smc.param.K * (std::pow(std::fabs(smc.s),smc.param.epsilon)) * fun + smc.error.tar_differential_second); //控制器计算,幂次趋近率
|
||
u = s->param->J * ( (-s->param->c * s->velocity.error) - s->param->K * s->s - s->param->K * (pow(fabs(s->s),s->param->epsilon)) * fun); //控制器计算,幂次趋近率
|
||
|
||
break;
|
||
case TFSMC:///tfsmc
|
||
|
||
if (fabs(s->position.error) - s->position.error_eps < 0){
|
||
s->position.error = 0;
|
||
return 0;
|
||
}
|
||
|
||
pos_pow = pow(fabs(s->position.error),s->param->q/s->param->p);
|
||
if(s->position.error<=0) pos_pow = -pos_pow;
|
||
|
||
|
||
s->s = s->param->beta * pos_pow + s->velocity.error; //滑模面
|
||
/* 饱和函数消除抖动 */
|
||
fun = Sat(s->s,s);
|
||
|
||
if(s->position.error!=0){
|
||
u = s->param->J * (s->position.tar_differential_second//目标值的二阶导 暂定是否删除，需测试
|
||
-s->param->K * s->s //s*K
|
||
-s->param->epsilon * fun //epsilon*SAT(S)
|
||
-s->velocity.error * ((s->param->q * s->param->beta) * pos_pow) / (s->param->p * s->position.error)); //控制器计算
|
||
}
|
||
else u = 0;
|
||
break;
|
||
case VELSMC:///比例积分滑模面，指数趋近律，速度控制
|
||
/* 滑模面 */
|
||
s->s = s->velocity.error + s->param->c * s->velocity.error_integral;
|
||
fun = Sat(s->s,s);
|
||
|
||
/*控制器计算，速度控制*/
|
||
u = s->param->J * (s->velocity.tar_differential - (s->param->c * s->velocity.error) - s->param->K * s->s - s->param->epsilon * fun);
|
||
break;
|
||
|
||
case EISMC:///比例积分滑模面，指数趋近律，位置控制
|
||
if (fabs(s->position.error) - s->position.error_eps < 0){
|
||
s->position.error = 0;
|
||
return 0;
|
||
}
|
||
/* 滑模面 */
|
||
s->s = s->param->c1 * s->position.error + s->velocity.error + s->param->c2 * s->position.error_integral;
|
||
fun = Sat(s->s,s);
|
||
/* 控制器计算,指数趋近率 */
|
||
u = s->param->J * ( (-s->param->c1 * s->velocity.error)- s->param->c2 * s->position.error - s->param->K * s->s - s->param->epsilon * fun);
|
||
s->s1= -s->param->c1 * s->velocity.error;
|
||
s->s2= -s->param->c2 * s->position.error;
|
||
s->s3= -s->param->K * s->s ;
|
||
s->s4= -s->param->epsilon * fun;
|
||
break;
|
||
}
|
||
/* 更新上一步的误差 */
|
||
s->position.error_last = s->position.error;
|
||
|
||
//控制量限幅
|
||
if (u > s->param->u_max)
|
||
{
|
||
u = s->param->u_max;
|
||
}
|
||
if (u < -s->param->u_max)
|
||
{
|
||
u = -s->param->u_max;
|
||
}
|
||
s->u = u;
|
||
return u;
|
||
}
|
||
|
||
|
||
|