一种基于FPGA的机器人实时速度规划方法

本技术属于运动控制，尤其涉及一种基于fpga的机器人实时速度规划方法。

背景技术：

1、如今机器人系统在工业领域有着广泛的应用场景，在流动生产线、金属加工等领域都有极为重要的作用。大量机器人设备都有快速运动的需求，同时要求速度曲线快速且平稳，这就需要一个合适的控制器和好的速度规划策略。其中多数都选择上位控制器+fpga的形式，并把部分处理过程安排在fpga里以实现更好的效果。

2、另外，现有规划算法通常涉及大量复杂函数的计算，有些规划算法会涉及三角函数、自然指数函数等复杂函数的计算，而在fpga内部实现这些复杂函数运算是有一定困难的，会面临fpga内部资源消耗大、求解时间长、精度有限等问题，上述原因导致当今市面上很多中低端控制器，只在fpga内实现较为简单的速度规划算法，限制了运行效果，甚至只让fpga负责发送脉冲，所有规划都在上位控制器完成，这样也降低了整体的运行效率。因此，在fpga内实现涉及复杂函数的速度规划算法，并达到资源消耗与运行效率的平衡，对机器人系统的发展有着重要意义。

技术实现思路

1、本技术的目的在于提供一种基于fpga的机器人实时速度规划方法，使在fpga内耗用较少的资源，得到更加柔顺的速度控制。

2、为了达到上述目的，本技术所采用的技术方案为：所述基于fpga的机器人实时速度规划方法，包括：

3、接收分段插补数据，得到插补数据，所述插补数据包括运动轴的移动位置增量；

4、根据插补数据计算加速段、匀速段和减速段的规划时间；

5、将匀速段、加速段和减速段的规划时间离散化为三段进给量，对插补数据中的移动位置增量进行判断，若移动位置增量小于设定阈值，则直接以最优时间轨迹规划进行插补，所述最优时间轨迹规划表现为梯形速度曲线，预规划结束；

6、若移动位置增量大于等于设定阈值，通过计算辅助修正函数进行二次规划后得到s型速度曲线，预规划结束；

7、将预规划后的速度曲线转换为脉冲输出用于控制电机。

8、进一步的，所述接收分段插补数据，得到插补数据，包括：

9、接收分段插补数据，将分段插补数据组成完整的插补数据包；

10、对完整的插补数据包进行解析，得到插补数据。

11、进一步的，所述根据插补数据计算加速段、匀速段和减速段的规划时间，包括：

12、判断插补数据是否符合预设条件，所述插补数据还包括运动轴的初始进给速度、终点进给速度、加速度、最大速度；

13、如符合预设条件，则将最大速度作为匀速段速度，并计算加速段、匀速段和减速段的规划时间；

14、如不符合预设条件，则根据移动位置增量、初始进给速度、终点进给速度和加速度计算约束下最大速度，再计算加速段和减速段的规划时间。

15、进一步的，所述预设条件为：

16、

17、其中，h表示移动位置增量，v0表示初始进给速度，v1表示终点进给速度，vmax表示最大速度，amax表示加速度。

18、进一步的，所述通过计算辅助修正函数进行二次规划后得到s型速度曲线，包括：

19、计算辅助修正函数，辅助修正函数用公式表示如下：

20、

21、根据辅助修正函数计算加速段和减速段第i个进给量所代表的速度，公式表示如下：

22、va[i]＝v0+(vv-v0)·fixedsigmoid[i]

23、vd[i]＝vv+(v1-vv)·fixedsigmoid[i]

24、其中，va[i]表示加速段第i个进给量所代表的速度，vd[i]表示减速段第i个进给量所代表的速度，v0表示初始进给速度，v1表示终点进给速度，vv表示约束下最大速度；

25、得到二次规划后的s型速度曲线。

26、进一步的，所述计算辅助修正函数，包括将x[i]离散化：

27、若二次规划的过程中的实时加减速度都不会超过电机执行机构能承受的最大范围，则：

28、

29、若二次规划的过程中会出现实时加减速度超过电机执行机构能承受的最大范围的情况，则：

30、

31、其中，x[i]为离散化后的第i个进给量所对应的规划值，amax为加速度，motoraccmax为电机执行机构能承受的最大加速度，n为加速段或减速段的进给总量，i为规划步骤中第i个进给量，和为校正因子，λ为偏移因子。

32、进一步的，所述计算辅助修正函数，还包括：

33、令x[i]＝xint[i]+xdeci[i]，其中，xint[i]为x[i]的整数部分，xdeci[i]为x[i]的小数部分；

34、第一部分：利用如下公式进行双步迭代计算xj1和yj1：

35、xk+2＝(1+dkdk+1·2-(2k+1))xk+(dk·2-k+dk+1·2-k-1)·yk

36、yk+2＝(1+dkdk+1·2-(2k+1))yk+(dk·2-k+dk+1·2-k-1)·xk

37、zk+2＝zk-dktanh-1(2-k)-dk+1tanh-1(2-k-1)

38、

39、其中，xk、yk、zk均为迭代变量，dk为方向因子，k∈[0,j1-1]，j1为第一部分迭代的最终迭代次数，tanh-1(2-k)由预置查找表提供；

40、根据如下公式初始化x0、y0、z0：

41、x0＝1/γ

42、y0＝0

43、z0＝-xdeci[i]

44、其中，γ为缩放因子；

45、在迭代完成后，根据如下公式计算：

46、

47、其中，lut(xint[i])表示根据xint[i]在小容量指数查找表lut(x)中找到对应的值；

48、第二部分：通过以下公式进行迭代计算：

49、xl+1＝xl

50、yl+1＝yl-dl(2-lxl)

51、zl+1＝zl+dl2-l

52、dl＝sign(yl)

53、根据如下公式初始化x'0、y'0、z'0：

54、x′0＝1+xj1+yj1

55、y′0＝1

56、z′0＝0

57、其中，xl、yl、zl均为迭代变量，dl为方向因子，l∈[0,j2-1]，j2为第二部分迭代的最终迭代次数，sign为sign()函数；

58、经过j2次迭代后，zj2就会收敛于fixedsigmoid[i]。

59、本技术与现有技术相比，其显著优点为：整体速度规划使用时间最优轨迹规划进行计算，并在其加减速阶段由基于cordic算法逼近的sigmoid函数进行二次规划，既克服了传统多段s型速度曲线计算量庞大且不适用在fpga内计算的缺点，同时又保留了s型速度曲线加速度连续的特点，可以减少插补过程中脉冲频率的突变，从而实现更好的插补效果，提升运动控制系统插补的稳定性和快速性。