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型加減速算法