电机加减速的时候需要用到平滑算法,常用的平滑算法有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