打印
[STM32F1]

STM32步进电机S型加减速算法

[复制链接]
7517|73
手机看帖
扫描二维码
随时随地手机跟帖
跳转到指定楼层
楼主
cry1109|  楼主 | 2020-3-24 15:58 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
本帖最后由 cry1109 于 2020-3-24 15:53 编辑

简单说明一下硬件资源,需要用到STM32两个定时器,TIM1产生PWM脉冲并对脉冲个数计数,TIM2开启定时中断用于算法的实现。采用CubeMX+Hal库配置,这里不做详细介绍,重点介绍S型加减速算法的实现。

首先了解一下S曲线函数,f(x)=1/(1+e^(-x)),这是S曲线最原始的函数,为什么叫“最原始的函数”,因为需要对其进行变换才能为我们所用,后面会详细介绍。取x∈[-10,+10]之间的所有整数在excel(是个好东西)里绘制S曲线图像,可以看到在x=-7和x=+7时f(x)分别接近最小值0和最大值1,所以我们就假定x=-7时f(x)min=0,x=+7时f(x)max=1 (使用过程中x=±7时已经满足我的精度要求,如果你需要更高的精度x可以取±8、±9...)。


图1:原始函数图像

在实际的过程中我们更希望x=0时f(x)=0,这样我们就能更好的把控程序,该怎么办呢?函数变换。我们高中时都学过把函数图像沿x轴平移、沿y轴平移、函数拉伸等等(在这里要感谢一位执教于高中数学的高中同学妹纸),上面讲到原始函数图像在x=-7时f(x)=0,如果把曲线图像向沿x轴向右平移7个单位,f(x)在x=0时不就等于0了?曲线图像向右平移几个单位那么x减几,向左平移几个单位那么x就加几,我们需要向右平移7个单位那么-(x-7)即可,得到一个新的函数f1(x):


图2:第一次变换

可以发现经过右移7个单位后得到的函数,当x=0时f1(x)的值等于原始函数在x=-7时的值,x=14时f(x)的值等于原始函数在x=7时的值,这样我们就把原始函数f(x)中x∈[-7,+7]的f(x)值域转移到经过第一次变换后得到的1f(x)  x∈[0,+14]的值域了,是不是离目标更近一步了?别急。

经过第一次变换后得到的f1(x)在x∈[0,+14]时,x取该区间里的15个整数f1(x)即可由最小值0按照S型轨迹增加到最大值1,当然这期间x也可以取值为0.1、0.2、0.3......13.7、13.8、13.9等等这样的小数。但是我们程序里的设计思路是按次加速,加速n次后频率要达到最大。“次”是整数,我们也不可能说加速1.1次、加速1.2次,当x∈[0,+14]时,经过14次加速即可加速至至最大,显然加速过程不够平滑。我们希望x在某个区间里可以取值100个、200个、任意个整数,f(x)都能够按照S型轨迹变化,这样加速次数就可以灵活控制,这时我们需要对f(x)做横向拉伸变换。

我们先看一下效果:

图3:  -0.5x+7

图3是x的系数除以2,得到的函数记为f2(x)。



图4:-2x+7

图4是x的系数乘以2,得到的函数记为f3(x)。

可以看到,f2(x)和f3(x)达到相同的最小值最大值时能够取得的x最大整数值分别为f1(x)的2倍、1/2倍。f1(x)在x=14时取得最大值、f2(x)在x=28时取得最大值、f3(x)在x=7时取得最大值,且最大值均相等。记f(x)=1/(1+e^(-ax+b)),那么我们是否可以认为当f(x)的值由0按照S型轨迹增至1时,能够取得x的整数个数为14*(1/a)个?OK,我们再来验证一下,取a=0.1,b=7,得到的函数记为f4(x):



图5:0.1x+7

果然,x能够取得最大的整数值为140,为f1(x)的x=14的10倍,也就是说我们可以加速140次至最大值,假设成立!

看到这儿是不是觉得胜利就在眼前了?最后我们再来优化一下,通过上诉变换我们可以得到14的整数倍数的最大加速次数,如果我们能得到1或者10的整数倍数的最大加速次数不是更好?很简单只需把前面讲到的-ax+b改为-(14/n)x+b,其中14/n = a,n为加速次数,例如我们想通过100次加速,那么函数即为f(x)=1/(1+e^((14x/100)+7)),我们前面首次变换时将函数图像沿x轴右移了7个单位( x∈[-7,+7] ),而14=7*2,数学是不是很美妙?有空可以验证一下如果向右平移8个单位、9个单位...,是不是可以将x的系数该为16/n、18/n?

在驱动步进电机过程中我们希望PWM脉冲频率按照S型轨迹变化,也就说f(x)的值对应PWM脉冲频率Freq。上诉函数中f(x)大小都在1以下,我们需要PWM脉冲频率达到几十k或者几百k。这这个简单呀,乘以倍数Freq=n*f(x),最大20kHz,就乘以20000、最大100kHz就乘以100000。

我们来看一个很漂亮的S曲线:



图6:理想的S曲线

按照上面讲的变换规则,最大PWM脉冲频率50kHz、经过500次加速至最大频率,生成的S曲线。

我们讲完了所有的S曲线变换规则,那么到底如何通过程序实现PWM脉冲频率按照S型曲线变换呢?

最初想通过公式计算得到本次的PWM脉冲频率值,算一次数据更新一下自动重载值,但是涉及到浮点数运算,对于STM32F103这样不带FPU的单片机来说计算一次数据需要100us+的时间,比较耗时。那么我们能不能提前算好数据,在调节频率时让单片机自己选择一个数据来更新重载值呢?好主意!


void CurveS_init(uint32_t *pbuff,uint32_t freq,int16_t count)
{
        int16_t i;

        for(i = 0;i<count;i++)
        {
            *pbuff++ = (uint32_t)ceil(((float)(freq-PWM_START_FREQ))/(1.0+exp((-i*14.0/count)+7.0)));
        }        
}

通过上面这个函数即可生成S曲线参数,函数的原型即为图6中的f(x)稍加变换。

pbuff:存储S曲线参数        freq:你需要的最大频率        count:你需要的加速次数

count越大,S曲线越平缓,加速过程越平滑,加速时间越长;count越小,S曲线越陡峭,加速过程越急剧,加速时间越短。

有了S曲线生成函数后,我们需要做的就是在程序初始化时生成一组S参数:

uint32_t CurveS_Para[500];

CurveS_init(CurveS_Para,50000,500);

这500个S曲线参数就是x∈[0,+500]之间取整时f(x)的值,把这500个点连接起来就成S型曲线。

有了S参数,我们就可以调速了,速度调节函数如下:

void SpeedAdjust(uint16_t count)
{
        Motor.Speed = CurveS_Para[count]*Motor.FrePropor/50+PWM_START_FREQ;    //计算本次速度
        
        htim1.Instance->ARR = TIM1_CLOCK_FREQ/Motor.Speed;
        htim1.Instance->CCR1 = (TIM1_CLOCK_FREQ/Motor.Speed)/2;
        
        if(Motor.Status == SPEED_INCREASE)            //加速
        {
                Motor.CountTemp++;
        }
        else if(Motor.Status == SPEED_DECREASE)       //减速
        {
                Motor.CountTemp--;        
        }
        Motor.Count = Motor.CountTemp*5/Motor.CountPropor;    //加速次数
}

count为加速次数,我们通过CurveS_Para[count]调用生成的S参数,Moto.FreqPropor为频率比例,MotorCountPropor为加速次数比例。由于我们是按照最大频率50kHz、最大加速次数500次(就是说加速500次频率可以达到50kHz)生成的S曲线,在实际使用过程中最大频率可能是任意值20kHz、30kHz等等,加速次数也可能是任意值200次、400次等等,但是我们只有一组S曲线参数,所以我们需要通过比例来对最大目标频率以及相应的加速次数做一下缩放。简单来说就是插值法,500个按照某个规律排列的数据中,每隔相同的间距取其中的一个数据组成一组新的数据,那么这组新数据应和原数据有相同的规律。

我们现在有一个数组CurveS_Para[500],里面的的元素按照S型函数规律排列,如果每隔5/3的间距在这个数组中据中取出一个元素然后组成一个新的数组,那么我们可以在原数组中取300个元素,且组成的新数组里的元素同样是按照S型函数排列。而速度调节函数中的5/Motor.CountPropor就是我们的取数间隔,如果300次加速至最大值Motor.CountPropor=3即可。本次调速需要生效的频率值同样需要通过比例计算得到 CurveS_Para[count]*Motor.FrePropor/50,如果我们最大的目标频率为30kHz,Motor.FrePropor=30即可,当加速次数count达到最大499时,速度同样达到最大。

速度调节函数中真正生效的是Motor.Count这个参数,在后面的调速中Motor.Count作为参数传递给SpeedAdjust()函数中。Motor.CountTemp是用来计算Motor.Count的一个中间变量,也是实际的已经加速的次。在加速过程中,每进行一次加速调用一次SpeedAdjus()函数,同时Motor.CountTemp++,记录加速次数。加速过程中频率按照S型规律由最小增到最大,对应的S参数CurveS_Para[0]增加到CurveS_Para[499]。而减速过程中频率同样按照S型规律由最大减至最小,参数CurveS_Para[499]减小到CurveS_Para[0],同时同时Motor.CountTemp--记录剩余的减速次数。简单的说,加速过中将CurveS_Para数组中的元素从前往后代入频率计算,减速过程中将CurveS_Para数组中的元素从后往前代入频率计算。OK,是不是很简单?

最后看一下速度调节状态机,电机状态由 加速->匀速->减速 变化,在加速和减速阶段调用速度调节函数。加速过程中加速次数达到最大后停止加速转为匀速运动,至于什么时候开始减速由你实际的程序决定。


void SpeedAdjustMachine(void)
{
        switch(Motor.Status)
        {
                /*加速*/                                                
                case SPEED_INCREASE:
                        if(Motor.Count <= COUNT_MAX)
                        {
                                SpeedAdjust(Motor.Count);
                        }
                        else
                        {
                                Motor.Status = SPEED_STABLE;
                        }
                        break;

                /*匀速*/        
                case SPEED_STABLE:
                        if(Motor.PWMcount >= (Motor.PWMneed-Motor.SpeedDecrPWM))
                        {               
                                Motor.Status = SPEED_DECREASE;        
                        }
                        break;
                                
                /*减速*/        
                case SPEED_DECREASE:
                        if(Motor.Count >= 0)
                        {
                                SpeedAdjust(Motor.Count);
                        }
                                                        
                        if(Motor.PWMover == 1)
                        {               
                                HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);
                        }
                        break;
                                
                default :
                        break;
        }
}

速度调节状态接在另一个定时器的定时中断中调用,多久调节一次速度由该定时器的中断频率决定,500us、1ms、2ms都行。中断频率和加速次数共同决定电机的加减速时间,需要根绝电机能够承受的最大加速度调节加速次数以及调速时间间隔。


最后再看一下实际的效果,每100us读取一次TIM1的自动重载值ARR,计算当前PWM脉冲,生成的S曲线图像:



通过示波器可以观察到频率由大减小,由小增大的一个过程。



使用特权

评论回复
评论
LNZXP 2021-5-14 15:46 回复TA
请问 楼主 你的程序可以用到直流电机上吗 
LNZXP 2021-5-14 15:43 回复TA
谢谢分享 
zlf1208 2020-10-10 15:27 回复TA
用cos函数也可以达到同样的目的 
评分
参与人数 1威望 +3 收起 理由
moticsoft + 3 很给力!
沙发
mmuuss586| | 2020-3-27 13:19 | 只看该作者

这个好,感谢分享

使用特权

评论回复
板凳
cry1109|  楼主 | 2020-3-27 14:48 | 只看该作者
mmuuss586 发表于 2020-3-27 13:19
这个好,感谢分享

找资料找的太累,干脆自己写一个。

使用特权

评论回复
地板
mmuuss586| | 2020-3-28 16:00 | 只看该作者
cry1109 发表于 2020-3-27 14:48
找资料找的太累,干脆自己写一个。

支持

使用特权

评论回复
5
wwchang| | 2020-3-31 13:05 | 只看该作者
不错,MARK一下

使用特权

评论回复
6
wakayi| | 2020-4-7 14:22 | 只看该作者
请问这种算法有什么好处

使用特权

评论回复
7
wowu| | 2020-4-7 14:24 | 只看该作者
在什么情况下适和采用这种算法呢

使用特权

评论回复
8
xiaoqizi| | 2020-4-7 14:25 | 只看该作者
这种算**不会太耗费资源啊

使用特权

评论回复
9
磨砂| | 2020-4-7 14:26 | 只看该作者
非常简洁明了

使用特权

评论回复
10
你瞅啥| | 2020-4-7 23:09 | 只看该作者
本帖最后由 你瞅啥 于 2020-4-7 23:16 编辑

这个指数公式在物理上有什么道理吗?我算S-Curve都是用梯形加速度来算出来的曲线。
对于大部分我接触过的应用来说,匀速段才是有效的,要求加速减速段尽可能的短,楼主的算法问题是收敛到匀速段似乎太慢了。




使用特权

评论回复
11
lihuasoft| | 2020-4-11 17:45 | 只看该作者
应给此贴作者颁奖,加精

使用特权

评论回复
12
gdszzyq| | 2020-4-14 10:23 | 只看该作者
我开始也是用这个思路去做的,后来直接在外面用EXCEL把定时参数计算好,再用数组把这些参数保存,用查表法更省事。

使用特权

评论回复
13
gdszzyq| | 2020-4-14 10:24 | 只看该作者
不明白为什么要用2个定时器,我一个定时器输出4路PWM

使用特权

评论回复
14
tcdbobo| | 2020-5-6 10:44 | 只看该作者
本帖最后由 tcdbobo 于 2020-5-6 11:06 编辑
cry1109 发表于 2020-3-27 14:48
找资料找的太累,干脆自己写一个。
请问,方便看一下您的这个关于 S曲线的 原始程序吗?我尝试移植到我的F410上,错误很多。不知道是哪里的问题。
       我用的是stm32f410rb ,也是采用CubeMX+Hal库配置的方式。用到的2个定时器一个TIM6中断定时 ,一个TIM9产生PWM,系统时钟为100M。
相关参数详见截图。您程序里的部分内容我已经相应修改。


PWM我是100M/10分频 计数最后设置为1kHz  
另一个计数器中断我改为100M/100分频  计数最后设置为2ms一次中断。


uint32_t CurveS_Para[500];
CurveS_init(CurveS_Para,1000,500);里我已经改为1000表示1kHz

TIM6.jpg (232.28 KB )

TIM6.jpg

TIM9 PWM 1KHz.jpg (237.87 KB )

TIM9 PWM 1KHz.jpg

keil.jpg (420.77 KB )

keil.jpg

时钟100M.jpg (214.65 KB )

时钟100M.jpg

使用特权

评论回复
15
daatyu| | 2020-5-7 16:00 | 只看该作者
不错,MARK一下

使用特权

评论回复
16
chongdongchen| | 2020-5-8 16:09 | 只看该作者
学习了,感谢分享

使用特权

评论回复
17
cry1109|  楼主 | 2020-5-8 16:15 | 只看该作者
本帖最后由 cry1109 于 2020-5-8 20:49 编辑
tcdbobo 发表于 2020-5-6 10:44
请问,方便看一下您的这个关于 S曲线的 原始程序吗?我尝试移植到我的F410上,错误很多。不知道是哪里的问 ...

这错误看看keil里有没有包含电机驱动部分代码的路径?有没有声明变量?晚上回去了我看一下。

使用特权

评论回复
18
cry1109|  楼主 | 2020-5-8 20:53 | 只看该作者
本帖最后由 cry1109 于 2020-5-13 17:12 编辑
tcdbobo 发表于 2020-5-6 10:44
请问,方便看一下您的这个关于 S曲线的 原始程序吗?我尝试移植到我的F410上,错误很多。不知道是哪里的问 ...

你用了一些没有定义的宏。代码里的TIM1_CLOCK_FREQ是我定义的一个宏,你的TIM9_CLOCK_FREQ好像没定义吧才报错的。你把这个文件解压一下,添加到你的工程里然后包含一下路径,没有声明的变量声明一下,应该就能用了。

MotorS.rar

1.79 KB

使用特权

评论回复
19
tcdbobo| | 2020-5-9 10:10 | 只看该作者
本帖最后由 tcdbobo 于 2020-5-9 15:31 编辑
cry1109 发表于 2020-5-8 20:53
你用了一些没有定义的宏。代码里的TIM1_CLOCK_FREQ是我定义的一个宏,你的TIM9_CLOCK_FREQ好像没定义吧才 ...
首先十分感谢代码的分享!!!
我把.h和.c文件分别加到我的工程中,相关的宏定义已修改,但是最终编译还是出现了,1个error,2个warning具体截图已作出标记。

error就是  "extern FLAG_TYPEDEF gFlag;"这个参数没有未知类型。我在.h文件中只找到了"MOTOR_TYPEDEF"的枚举。

warning好像是说math.h里的"ceil"是个double的数,但是实际我看您的程序中已经做了强制转换“(uint32_t)ceil(((float)....",顺便说一下,我在
main.c里#include "math.h"的头文件。但后来试了一下,不加最后编译还是一样的warning提示。
具体截图已作出标记。请教一下。





math文件.jpg (152.26 KB )

math文件.jpg

提示.jpg (381.7 KB )

提示.jpg

使用特权

评论回复
20
cry1109|  楼主 | 2020-5-10 08:34 | 只看该作者
tcdbobo 发表于 2020-5-9 10:10
首先十分感谢代码的分享!!!
我把.h和.c文件分别加到我的工程中,相关的宏定义已修改,但是最终编译还是 ...

你程序里用不到的变量可以删掉,比如所那个extern FLAG_TYPEDEF gFlag,如果移植过程中你发现有变量未定义的报错,你可把用不到的变量删掉或者自己重新定义变量替换掉。math.h需要包含到motor_cntorl.c里。

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

40

主题

172

帖子

4

粉丝