首页 > 代码库 > stm32S型加减速算法

stm32S型加减速算法

电机加减速的时候需要用到平滑算法,常用的平滑算法有S型跟梯形,因为S型的平滑效果比较好,所以选择S型。

看了几篇论文,有的是使用多项函数、有的是使用分段,但这两个的函数曲线看起来并不是那么好,后面选择了sigmoid这类S型非线性变换。

sigmoid函数 y = 1/(1+exp(-in))是一个良好的阈值函数,函数连续、光滑,严格单调并关于(0,0.5)中心对称,函数值∈[-1.1],

其导数f‘(x)=f(x)*[1-f(x)],可以节约计算时间

技术分享

sigmoid函数曲线的的方程y = 1/(1+exp(-in)),在[-6,6]的图形如上所示:

做sigmoid变换的目的是把(-inf,+inf)取值范围的信号(用x表示)映射到(0,1)范围内(用y表示):y=h(x)。

 

matlab代码:

>> X = -6 : 0.1 : 6;
Y = sigmoid(X);
h = figure;
plot(zeros(size(X)), Y, Color, r, LineWidth, 2);
hold on;
plot(X, Y, Color, b, LineWidth, 2);
set(gca, XTick, -10 : 0.5 : 10);
set(gca, YTick, 0 : 0.05 : 1);
set(gca, FontSize, 7);
grid on;
fontSize = 10;
title(Sigmoid Funtion: Y = 1 / (1 + exp(-X)), FontSize, fontSize);
ylabel(Y, FontSize, fontSize);
xlabel(X, FontSize, fontSize);
set(gcf, PaperType, A4);
print(h, -dpng, sigmoid.png);




>> X = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20];
 X = [-4 -3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6];
 Y1=[0.018  0.029  0.047  0.076  0.119  0.182  0.269  0.377     0.500  0.622 0.731  0.817  0.881, 0.924  0.952  0.970  0.982  0.989  0.993  0.996  0.997    ];
Y2=[0.0179862    0.0293122    0.0474259    0.0758582    0.119203    0.182426    0.268941    0.377541    0.5        0.622459    0.731059    0.817574    0.880797    0.924142    0.952574    0.970688    0.982014    0.989013    0.993307    0.99593        0.997527];
 figure(1),scatter(X,Y1),hold on;

 技术分享

 

 把函数Y的值保存到b.txt中,地址在D:\Documents\MATLAB
X = [-6 -5 -4 -3 -2 -1 0 1 2 3 4 5 6 ];
Y = sigmoid(X);fid=fopen(‘b.txt‘,‘wt‘);
fprintf(fid,‘%g\t‘,Y);
fclose(fid);

Y = sigmoid1(X);
address=‘F:\MATLABdata\‘;
fid=fopen([address,‘c.txt‘],‘wt‘);
fprintf(fid,‘%g\t‘,Y);
fclose(fid);

 

 y = 1/(1+exp(-in))在[-5,6]上面的是个解弄出来,后函数用查表法

float   Sigmoid_Curve_val[] ={
                0.018, 0.029, 0.047, 0.076,0.119,    0.182,    

                0.269, 0.377,    0.500, 0.622,    0.731, 0.8178,    

                0.881, 0.924,    0.952, 0.970,    0.982, 0.989,    

                0.993, 0.996,    0.997    };
int s_pluse = 0;
int i = 0 , j = 0;
float fpTargetR_Speed_old = 0 , fpCurrentR_Speed_old = 0;
float fpTargetL_Speed_old = 0 , fpCurrentL_Speed_old = 0;
float getR_s_data[20],getL_s_data[20];
                
    
/*******************************************************************
函数名称:Motor_speed_plan
函数功能:从当前的速度变化到目标速度时缓慢的变化
输入:PID结构体指针
输出:无
备注:
********************************************************************/
void Motor_speed_plan(ST_ROBOT_MOTOR_SPEED_PLAN* pstSpeedPlan , ST_ROBOT_MOTOR* pstMotor)
{            
            if(fpTargetR_Speed_old != pstMotor->stMotorR.fpTarget_Speed)//上位机来一次数据就更新 就是记录这时候电机的速度   //fpTarget_Speed_old在哪里赋值
            {
                    fpTargetR_Speed_old = pstMotor->stMotorR.fpTarget_Speed;
                    fpCurrentR_Speed_old = pstMotor->stMotorR.fpCurrent_Speed;        //记录新目标速度来时电机的速度
                    i= 0;
            }
            //if(pstMotor->stMotorR.fpCurrent_Speed != pstMotor->stMotorR.fpTarget_Speed)//当前速度 ≠ 目标速度
            if(i < 20)//当前速度 ≠ 目标速度
            {                                
                    pstMotor->stMotorR.fpCurrent_Speed = fpCurrentR_Speed_old + (pstMotor->stMotorR.fpTarget_Speed - fpCurrentR_Speed_old) * Sigmoid_Curve_val[i++];
                    getR_s_data[--i] = pstMotor->stMotorR.fpCurrent_Speed;
                    i++;
            }

            
            if(fpTargetL_Speed_old != pstMotor->stMotorL.fpTarget_Speed)//上位机来一次数据就更新 就是记录这时候电机的速度   //fpTarget_Speed_old在哪里赋值
            {
                    fpTargetL_Speed_old = pstMotor->stMotorL.fpTarget_Speed;
                    fpCurrentL_Speed_old = pstMotor->stMotorL.fpCurrent_Speed;        //记录新目标速度来时电机的速度
                    j= 0;
            }
            //if(pstMotor->stMotorR.fpCurrent_Speed != pstMotor->stMotorR.fpTarget_Speed)//当前速度 ≠ 目标速度
            if(j < 20)//当前速度 ≠ 目标速度
            {                                
                    pstMotor->stMotorL.fpCurrent_Speed = fpCurrentL_Speed_old + (pstMotor->stMotorL.fpTarget_Speed - fpCurrentL_Speed_old) * Sigmoid_Curve_val[j++];
                    getL_s_data[--j] = pstMotor->stMotorL.fpCurrent_Speed;
                    j++;
            }
}

 

 

为什么我们喜欢用sigmoid这类S型非线性变换?

http://www.52cs.org/?p=363

http://blog.csdn.net/itplus/article/details/11496595

http://computing.dcu.ie/~humphrys/Notes/Neural/sigmoid.html

http://blog.csdn.net/kyu_saku/article/details/44422519

 

matlab中fopen函数与fprintf用法

 

 http://blog.sina.com.cn/s/blog_4b986f1a0101349k.html

stm32S型加减速算法