伺服电机控制算法疑问 (amobbs.com 阿莫电子技术论坛) -

huali8412 发表于 2016-9-19 18:03:57

伺服电机控制算法疑问

我本人是从事单片机程序开发的，以前从来没有接触过运动控制这一块领域。目前，公司有一个项目要用到伺服电机，驱动器的电路和代码都有，领导让我从代码里研究一下驱动器的算法，看了几天，看到头晕晕的，还没有一个大概的概念。被一个函数卡了好几天还是没想明白，我贴一下代码出来，希望多多指教。
/**
* @paramdis: the distant to move.
* @paramt: the given time to move.
* @paramacc: accerlation
* @retval 0 for no error. 1 for time to short or acc to small;
*/
u8 setTPath(int64_t dis,int64_t t,int acc)
{
int64_t temp,t_destin,treferencepos;
int dec;
vel=AV*10;//AV为两次电机转过的角度差（脉冲数量表示）
treferencepos=getPosition();//电机实际转过的角度，也是脉冲数量表示

   t_destin=dis;
dis=1000*(dis-treferencepos);

t=10*t;
if(vel>0)
{
dec=-acc;
if(vel*t-vel*vel/acc/2>dis)
{
   if(vel*vel/acc/2>dis)
dec=acc;
   acc=-acc;
}
}
else
{
dec=acc;
if(vel*t+vel*vel/acc/2>dis)
{
acc=-acc;
}
else
{
   if(vel*vel/acc/2<dis)
dec=-acc;
}

}
if(dec/acc==-1)
{
temp=t*t+2*vel*t/acc-4*dis/acc+3*vel*vel/acc/acc;

if(temp<0)
return 1;
temp=sqrt(temp);
acctime=(t-vel/acc-temp)/2;
dectime=t-(acctime+vel/acc);

if(acctime>0&&dectime>acctime&&dectime<t)
;
else
{
acctime=(t-vel/acc+temp)/2;
dectime=t-(acctime+vel/acc);
if(acctime>0&&dectime>acctime&&dectime<t)
;
else
return 2;
}
}
else
{
acctime=(dis-vel*t-vel*vel/2/acc)/(vel+acc*t);
dectime=t+vel/acc+acctime;
}
destin=t_destin;
accelerate=acc;
decelerate=dec;
maxvel=vel+acc*acctime;
endtime=t;
distant=dis;
referencepos=treferencepos;
return 0;
}

该算法的核心应该就是位置控制算法，代码中的公式temp=t*t+2*vel*t/acc-4*dis/acc+3*vel*vel/acc/acc;代表什么意义，怎样推导出来的，算来算去都不行，梯形算法还是S型算法我都推导不出来，麻烦了解的大侠给我指导一下

霸气侧漏 发表于 2016-9-19 18:20:58

我做过运动控制，回头看看你的程序

bsz84 发表于 2016-9-19 19:38:30

想看看运动控制程序，看你贴出的程序不明白。

wkman 发表于 2016-9-19 19:50:14

{:titter:} 是DSP 程序么？？？{:shocked:}

huali8412 发表于 2016-9-20 08:53:27

wkman 发表于 2016-9-19 19:50
是DSP 程序么？？？

不是DSP，是stm32的程序

huali8412 发表于 2016-9-20 08:54:18

霸气侧漏发表于 2016-9-19 18:20
我做过运动控制，回头看看你的程序

有空麻烦帮我看看，有什么不明白直接说

knight_sh 发表于 2016-9-20 09:11:43

我觉得先不要深入代码，先把原理搞清楚。这个应该是T型速度规划吧，根据速度，时间，距离公式换算，只不过这里面距离可能由伺服旋转角度之类的作了替换，意思都是一样。

huali8412 发表于 2016-9-20 09:17:38

knight_sh 发表于 2016-9-20 09:11
我觉得先不要深入代码，先把原理搞清楚。这个应该是T型速度规划吧，根据速度，时间，距离公式换算，只不过 ...

恩，我现在就是研究这个算法的原来，这个算法大概是这样的：先给定位移，时间，然后去算加速时间，匀速时间和减速时间。把参数算出来之后就开始运动。但是中间有些公式不知道怎么得来的。这些参数最后都是以脉冲数量去表示，编码器是1000线的。

whatcanitbe 发表于 2016-9-20 09:58:03

huali8412 发表于 2016-9-20 09:17
恩，我现在就是研究这个算法的原来，这个算法大概是这样的：先给定位移，时间，然后去算加速时间，匀速时 ...

有时间帮你看看。
可以换个思路，从已知推结果不好直接推的话。

你可以，找几个测试点，看下函数的输出是什么样的，然后自己绘制下图形。大概就能明白了。

huali8412 发表于 2016-9-20 10:24:11

whatcanitbe 发表于 2016-9-20 09:58
有时间帮你看看。
可以换个思路，从已知推结果不好直接推的话。

整个工程涉及很多参数，这算法里面带有PID算法，位置环->速度环->电流环，然后在电流环输出互补的PWM。工程还有一个表格数组，存放一组数据，暂且定义DATA,数据大小为720，这个应该是电角度，以为电机的极数是2对，数据的规律前半部分是从0增大到1000然后从1000减到0，后半部分从0一直到-1000然后从-1000到0，我猜这跟编码器有关系，因为编码器的线数是1000线的。因为我以前从来没有接触过伺服电机，没法从这些分散的数据找出联系。

huali8412 发表于 2016-9-20 10:29:11

霸气侧漏发表于 2016-9-19 18:20
我做过运动控制，回头看看你的程序

算法可能是增量PID算法，然后里面包含S曲线的平滑处理，可是始终不知道公式怎么来的，有相关资料可以提供一下吗？谢谢

zouzhichao 发表于 2016-9-20 10:31:24

楼主，你该怎么谢我？

C1060825815 发表于 2016-9-20 10:35:04

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

我先谢谢你，正在做STM32运动控制这块，这个正好用得上。。。。

huali8412 发表于 2016-9-20 10:45:10

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

一下子茅塞顿开，想了好几天了，真的是非常感谢你的解疑,我还有好几个小疑问想请教你，可以加你QQ吗？

zouzhichao 发表于 2016-9-20 11:13:16

C1060825815 发表于 2016-9-20 10:35
我先谢谢你，正在做STM32运动控制这块，这个正好用得上。。。。

哈哈，有人用得上就算没白费功夫

knight_sh 发表于 2016-9-20 11:43:07

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

哈哈，我正要画图...{:lol:}

zouzhichao 发表于 2016-9-20 11:48:21

knight_sh 发表于 2016-9-20 11:43
哈哈，我正要画图...

哈哈，握个爪…

zouzhichao 发表于 2016-9-20 11:59:22

knight_sh 发表于 2016-9-20 11:43
哈哈，我正要画图...

跟你的思路是一样的
1，有个sqrt函数，首先想到这是一个一元二次方程求解
2，没有出现别的复杂函数，说明这肯定不是s加速，基本是梯形/三角形的，因为匀加速s = v0*t+0.5*a*t*t，换位思考，如果是我去做，我会去解决什么样的问题，我肯定先猜测梯形运动规划，毕竟这个在伺服中应用太广了，结果按照梯形图推导，和楼主的公式一模一样，至此一目了然

huali8412 发表于 2016-9-20 15:38:16

zouzhichao 发表于 2016-9-20 11:59
跟你的思路是一样的
1，有个sqrt函数，首先想到这是一个一元二次方程求解
2，没有出现别的复杂函数，说明 ...

这种梯形算法是不是要判断几种情况啊，我看函数体里面对输入的参数进行了好几次的判断，是不是判断有没有加速过程或者匀速过程？

zouzhichao 发表于 2016-9-20 15:58:54

huali8412 发表于 2016-9-20 15:38
这种梯形算法是不是要判断几种情况啊，我看函数体里面对输入的参数进行了好几次的判断，是不是判断有没有 ...

实际上可能不需要判断那么多，多虑了

huali8412 发表于 2016-9-20 16:01:49

zouzhichao 发表于 2016-9-20 15:58
实际上可能不需要判断那么多，多虑了

是否只考虑正常情况就可以了，不需要判断额外的情况！因为真正应用的时候我们还要去调试参数的。不知道我的想法对不对！

whatcanitbe 发表于 2016-9-21 09:24:01

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

大哥是人才啊

StephenCui 发表于 2017-1-2 20:33:10

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

S1等式应该再加上X乘以V0

kinsno 发表于 2017-1-2 22:05:48

huali8412 发表于 2016-9-20 09:17
恩，我现在就是研究这个算法的原来，这个算法大概是这样的：先给定位移，时间，然后去算加速时间，匀速时 ...

所以看到时间+位移，或时间+速度，这种类型基本都是T形加速。。。三角加速印象中不是这样子的。。。
一般来说无所谓就是3种：T型加速与其变种三角，还有一种是三角函数加速，还有一种是PID及各种变态PID。。。
实际上来说，真正应用中，应该是使用PID的。。。

vc9181 发表于 2017-1-2 22:06:02

zouzhichao牛人

zouzhichao 发表于 2017-1-2 22:35:42

StephenCui 发表于 2017-1-2 20:33
S1等式应该再加上X乘以V0

我发的那个公式推导有些错误，后来又推导了一个，有时间再找出来

zouzhichao 发表于 2017-1-2 22:36:35

vc9181 发表于 2017-1-2 22:06
zouzhichao牛人

惭愧，发了个有错误的

FireHe 发表于 2017-1-3 01:26:41

zouzhichao 发表于 2017-1-2 22:36
惭愧，发了个有错误的

最近也要搞伺服相关的产品，大师你发的那个公式推导我没看明白，有空能仔细讲讲吗？

zouzhichao 发表于 2017-1-3 09:35:00

FireHe 发表于 2017-1-3 01:26
最近也要搞伺服相关的产品，大师你发的那个公式推导我没看明白，有空能仔细讲讲吗？

...

zouzhichao 发表于 2017-1-3 09:53:52

StephenCui 发表于 2017-1-2 20:33
S1等式应该再加上X乘以V0

zouzhichao 发表于 2017-1-3 09:55:42

故事重提，怕之前那个图误导大家，又把那个公式推倒了一遍，用了两种方法

su33691 发表于 2017-1-3 09:57:11

多谢楼上的讲解。

浮生莫若闲 发表于 2017-1-3 13:41:07

zouzhichao 发表于 2017-1-3 09:55
故事重提，怕之前那个图误导大家，又把那个公式推倒了一遍，用了两种方法 ...

感谢赐教{:smile:}

Xiyuhexuan 发表于 2017-1-3 15:06:56

呵呵牛叉！！！！

Xiyuhexuan 发表于 2017-1-3 15:10:37

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

呵呵呵收藏！谢

zouzhichao 发表于 2017-1-3 15:14:23

Xiyuhexuan 发表于 2017-1-3 15:10
呵呵呵收藏！谢

12楼的是错的
后面更新的才是对的

Xiyuhexuan 发表于 2017-1-3 15:29:50

zouzhichao 发表于 2017-1-3 15:14
12楼的是错的
后面更新的才是对的

嗯看到了看来您也是做这个行业的哈！

zouzhichao 发表于 2017-1-3 15:33:27

Xiyuhexuan 发表于 2017-1-3 15:29
嗯看到了看来您也是做这个行业的哈！

我不是这个行业的，我layout民工一枚

Xiyuhexuan 发表于 2017-1-3 15:36:54

zouzhichao 发表于 2017-1-3 15:33
我不是这个行业的，我layout民工一枚

哈哈只能膜拜了！

wenxiaofu123 发表于 2017-1-3 17:33:05

zouzhichao牛人

jetta2014 发表于 2017-1-4 10:55:56

zouzhichao 发表于 2017-1-3 09:55
故事重提，怕之前那个图误导大家，又把那个公式推倒了一遍，用了两种方法 ...

点赞一下！

windancerhxw 发表于 2017-1-4 18:54:49

{:handshake:}点赞一个，厉害

marshallemon 发表于 2017-1-4 20:03:22

STM32哪个系列的？

jiangtc 发表于 2017-3-19 21:23:39

不错啊，最近也在弄伺服电机

powerlabor001 发表于 2017-3-20 04:24:20

高人太多了，^_^，mark一下。

XiaoMin553694 发表于 2017-3-21 18:43:24

acctime=(dis-vel*t-vel*vel/2/acc)/(vel+acc*t);中acc*t是代表啥意思？t不是完成位移的总时间吗？

529063382 发表于 2017-3-21 20:16:59

zouzhichao 发表于 2016-9-20 10:31
楼主，你该怎么谢我？

程序能执行这个算法吗？？？计算时间大概需要多少

zouzhichao 发表于 2017-3-21 20:46:28

529063382 发表于 2017-3-21 20:16
程序能执行这个算法吗？？？计算时间大概需要多少

肯定能啊，如果m4带fpu的执行起来速度不会慢

XiaoMin553694 发表于 2017-3-21 20:51:41

zouzhichao 发表于 2017-3-21 20:46
肯定能啊，如果m4带fpu的执行起来速度不会慢

请教一下，acctime=(dis-vel*t-vel*vel/2/acc)/(vel+acc*t);中acc*t是代表啥意思？{:handshake:}

aisledianzi 发表于 2019-4-21 15:24:54

不错,留存

xinchangkk 发表于 2019-4-22 08:48:02

@大家

有没有这方面的书籍？

wulixian 发表于 2019-8-7 20:46:16

不错啊，最近也在弄伺服电机

limaotaizi 发表于 2019-8-7 23:26:47

学习了。。。~~

a515509429 发表于 2020-8-31 09:33:27

学习学习了

lnso 发表于 2020-8-31 19:29:07

能搞电机控制算法的都是牛人

TINXPST 发表于 2020-8-31 23:20:15

学习了

麦田有怪圈 发表于 2020-9-1 14:00:19

厉害了，我的坛友

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

伺服电机控制算法疑问