CN107065784A

CN107065784A - 用于直角坐标机器人在高速运动中实现在线多级调整方法

Info

Publication number: CN107065784A
Application number: CN201710322406.5A
Authority: CN
Inventors: 邬惠峰; 黄俊杰; 吴瑜冰
Original assignee: Hangzhou Dianzi University
Current assignee: Hangzhou Dianzi University
Priority date: 2017-05-09
Filing date: 2017-05-09
Publication date: 2017-08-18

Abstract

本发明公开了一种用于直角坐标机器人在高速运动中实现在线多级调整方法。方法提出了这一种基于非对称、多内核的运动控制板及机器视觉的硬件条件下以自适应的速度曲线规划、基于变速的插补控制及自适应的模糊PID反馈控制相结合的控制方法，实现了CNC系统的在线调整运动参数，在提高了系统的响应实时性的同时，保证了系统在多次预测后进行逐步调整时的鲁棒性。本发明解决了普通控制方法的参数固定，无法做到运动的终点位置发生突变后的运动状态在线实时更新及发生终点位置突变后系统会产生较大抖动的问题，旨在提高运动控制的实时性和环境适应性，可衍伸应用于各类需要随着感知结果变化要进行在线的实时更新控制的工作环境。

Description

用于直角坐标机器人在高速运动中实现在线多级调整方法

技术领域

本发明涉及在CNC系统中加入一种用于直角坐标型机器人在高速运动过程中实现在线多级调整的方法，属于电机运动控制领域，可提供一种实现在高速运动中保证系统实时性及鲁棒性的电机高速运动控制方法，适应各类需要随着感知结果变化要进行在线的实时更新控制领域的需要。

背景技术

长期以来，我国的大部分带视觉控制的机器人都由国外引进，核心技术基本被国外品牌垄断，并且对于机器人的研究大部分是针对于装置结构较为复杂，制造成本相对较高的机械臂，对于直角坐标型机器人的研究相对较少。随着自动化控制的要求越来越高，对于传统机器人提出了更高的要求。越来越多的企业需要精度高、成本低、环境适应能力和可维护性强的机器人投入工业生产中去。相对于机械臂而言，直角坐标型机器人满足了成本低以及可维护性强的要求，但一般的直角坐标型机器人主要以定点往复运动为主，无法实现当出现运动终点位置发生改变时的参数实时在线调整，无法满足日益复杂的生产环境需求，同时传统的直角坐标型机器人在高速运动过程中的稳定性也无法保证。解决该问题的方法之一是在直角坐标型机器人的基础上结合机器视觉以提高对于动态环境的适应性，如德国百格拉公司的直角坐标型机器人与德国Vision Components公司的智能相机相结合的系统在工业上的成功运用。但是，用上述的系统需要从国外引进，需要支付高昂的购买费用和授权费用，无法被我国量大面广的中小型企业所使用。

因此我们提出了一种用于直角坐标型机器人在高速运动过程中实现在线多级调整的方法。在现有方法中，传统的基于时间分割的插补方法以及自适应的模糊PID在国内外已有论述，但传统的时间分割插补法无法适应在一次运动过程中轨迹的多次在线的实时调整，而自适应的模糊PID主要用于温控系统，如本文针对直角坐标型机器人在高速运动过程中实现实时在线调整所运用的以自适应的速度曲线规划、基于变速的插补控制及自适应的模糊PID反馈控制相结合的控制方法尚未出现。同时该方法的特点在于可以对运动过程的变化进行自适应调节，提高了直角坐标型机器人的环境适应性和稳定性，可衍生并运用在例如工件定位，分类搬运等的工业生产中。

发明内容

本发明的目的在于针对现有的控制方法的参数固定，无法做到运动的终点位置发生突变后的运动状态在线实时更新及发生终点位置突变后系统会产生较大抖动的问题，提供了一种基于机器视觉的，在CNC系统中加入一种用于直角坐标型机器人在高速运动过程中实现在线多级调整的方法，保证了系统在高速运动过程中发生了终点位置突变后，可以对运动参数进行在线的实时更新，并保证系统在突变前后的平稳过渡，以达到适应不同环境的控制流程的需要。

本发明的目的是通过以下技术方案来实现的：在非对称、多内核的控制器存储空间内划分了一个与工控主机通信所专用的内存空间，用于存放实时更新的终点位置坐标。在控制器内增加了运动控制算法优化模块，实现了以自适应的速度曲线规划、基于变速的插补控制及自适应的模糊PID反馈控制相结合的控制方法。

用于直角坐标机器人在高速运动中实现在线多级调整方法，该方法为在传统CNC系统中以非对称、多内核的控制器为基础，增加了运动控制算法优化模块，实现了以自适应的速度曲线规划、基于变速的插补控制及自适应的模糊PID反馈控制相结合的控制方法。

所述的非对称、多内核的控制器，划分一块与工控主机通信所专用的内存空间，用于存放实时更新的终点位置坐标；同时采用了以IPC通信机制、MSGRAM为基础，结合了中断情况判断、划分数据修改请求动态优先级的拥有时效性保证的变量共享机制，用于在非对称、多内核的核间通信方面确保实时系统的稳定性。

所述的运动控制算法优化模块，用于获取实时的运动位置坐标及进行速度曲线规划，进行插补运动并实现反馈控制。

所述的自适应的速度曲线规划控制，通过倍频采样的方式从指定内存中获取实时的运动终点位置坐标，从而通过经优化后的速度规划方法，得到自适应的、实时的速度变化曲线。一旦发现两次采样得到的终点坐标差值大于阈值，则认为发生了终点变化，需要实时更新速度曲线。

所述的优化后的速度规划方法，具体包括以下步骤：

步骤(1)、初始化得到每条轴的电机所处的位置坐标、需要运动到达的终点坐标、目前电机的运动速度V_S、电机的最大速度V_max及最大加速度A_max；获得终点位置坐标以及起始坐标后，得到各电机所需运动的距离S；由于目标轴的负载、可运动距离、电机功率的影响，会造成不同目标轴的电机所能达到的最大速度和最大加速度不同，因此需要在每次运动控制开始时进行测量和确定；

步骤(2)、获得每条轴的电机所能达到的最大进给率V_max，求得并初始化S型速度曲线中加加速时间T1，匀加速时间T2，减加速时间T3，加减速时间T5，匀减速时间T6，减减速时间T7为：

步骤(3)、在得出各个时间段的初始值后，得到对应加加速、匀加速、减加速、匀速、加减速、匀减速、减减速阶段点的速度公式为：

v₂＝v₁+Jerk*T₁*T₂ T₁≤t＜T₂ 公式(6)

v₄＝v₃ T₃≤t＜T₄ 公式(8)

v₆＝v₅-Jerk*T₅*T₆ T₅≤t＜T₆ 公式(10)

若要求各阶段内某一时刻的速度只需将对应时间改为电机运行时间t；

步骤(4)、在求出各阶段时间后，得到在理想状态下不包括匀速阶段的位移S为：

步骤(5)、将不包括匀速阶段的位移S与实际所需要运动的距离进行比较，判断是否存在匀速阶段；具体情况可分为①完整的七段S型速度曲线；②不含匀速段的S型速度曲线；③不含匀速段和匀减速段的S型速度曲线；④不含匀减速段的S型速度曲线；⑤基本四段速度曲线；⑥不含匀加减速的S型速度曲线；⑦不含匀加速和匀速段的S型速度曲线；

步骤(6)、在确定了一次运动周期的速度规划曲线后，仍旧对指定存储内存进行定时采样，若两次所得的终点位置坐标差值大于设定阈值，则认为已发生终点位置变化，重新启动规划任务，将当前的位置及速度作为初始化数据输入，重新规划新的运动速度曲线；若相差在阈值范围内，则只需将对应时间输入，可迅速输出速度，缩短运算时间。

所述的自适应的模糊PID控制，引入了周期性采样的环节，保证了当发生速度改变时，可以较快地将其引入模糊PID控制器中进行运算。自适应的模糊PID方法在保证运算效率的前提下，减少了普通PID方法的超调现象。

本发明的有益效果如下：

(1)通过采用基于非对称、多内核的控制器，实现了将周期性采样，前馈控制与速度曲线规划及模糊PID相结合的自适应方法，达到了毫秒级的自适应水平，用于解决现有的控制方法的参数固定，无法做到运动的终点位置发生突变后的运动状态在线实时更新及发生终点位置突变后系统会产生较大抖动的问题。

(2)非对称、多内核的运动控制器结构设计新颖，本着利用最少的硬件资源以获得最高效的、最快速的信息传输为目的进行设计，构建多路并行通道以增强控制器交互的实时性，控制器留有多处测试点和拓展引脚使该控制器具有广泛拓展应用性。

附图说明

图1为本发明一种算法驱动电路控制器的结构框图

图2为本发明一种基于时间分割法的插补示意图

图3为本发明一种简单的模糊PID结构设计示意图

图4为本发明一种针对直角坐标型机器人的新型电机控制算法软件流程图

具体实施方式

以下将结合附图对本发明作进一步说明。

为了克服现有技术的缺陷，本发明提出了一种支持实现本发明中所述控制算法的非对称、双内核的电机控制器。参见图1，为本发明所采用控制器的结构框图，包括供电模块、稳压模块，主系统电路，通信模块、电机驱动电路以及附属外设VGA显示、PC端上位机，其中通信模块用于数据的上传存储，主系统电路用于控制调配硬件资源，电机驱动电路用于操控电机。

所述的电源模块考虑到要具有足够大的驱动电流，优选的，我们选用工业用多路220V转24V开关电源。该24V输送至稳压模块。

所述的稳压模块实现了将直流24V电压降压到12V、5V以及3.3V。将各电平输送至主系统电路中给各自需求的芯片供电。

所述的主系统电路目的实现数据的交互以及执行相关控制指令，对实时和高速要求较高，优选的，采用F28M35为主核心芯片进而搭建主系统电路。并且该系统电路通过RS232与VGA显示屏幕相连，所述VGA显示屏幕用于显示小球轨迹与控制平台。该系统电路还通过CAN总线与通信模块相连，所述通信模块为一转换器，并与PC端上位机相连，所述上位机将坐标数据通过USB总线传输给通信模块，通信模块将其格式转换后传输给系统电路。系统电路内部产生PWM电机驱动脉冲，通过适当接口将其传输给电机驱动电路，所述电机驱动电路用于驱动电机。

为支撑所述本发明目的控制算法的实现，构建了核心电机驱动电路，应用了一款具有共享内存的异步多核主控芯片，实现了基于IPC和MSGRAM的多核协作机制，多核结构上应用了主从拓扑结构，完美结合控制与计算寄存器目的使代码执行效率得到明显提高。多核芯片其中的ARM核主控制，主要起监测和开关作用，负责描述此时的电机状态并以此为依据根据本发明算法决定进一步的控制命令，另外该核还负责坐标数据的接收缓存。DSP核负责处理这种控制命令，为实施者，ARM核发出的命令和提供的基础数据通过多核芯片的一块共享内存区间进行与DSP核数据的快速交互，结合DSP核负责实现的反馈脉冲的精确计数，控制脉冲的精确输出，为本发明算法的具体实现提供了最优的硬件支持。

参见图4，本发明的另一个目的是针对运动控制的高速自适应的速度曲线规划、高速自适应的运动插补控制及自适应的模糊PID反馈控制等三个方面进行了研究，并提供了一种对高速插补运动过程中发生了终点位置突变时提高系统鲁棒性的方法。应用上述系统的算法实现包括如下步骤：

步骤(1)、初始化得到每条轴的电机所处的位置坐标、需要运动到达的终点坐标、目前电机的运动速度V_S以及电机可以达到的最大速度V_max及最大加速度A_max等数据。获得终点位置坐标以及起始坐标后，可以得到各电机所需运动的距离S。由于目标轴的负载、可运动距离、电机功率等因素的影响，会造成不同目标轴的电机所能达到的最大速度和最大加速度不同，因此需要在每次运动控制开始时进行测量和确定。

步骤(2)、获得每条轴的电机所能达到的最大进给率V_max，可以求得并初始化S型速度曲线中加加速时间T1，匀加速时间T2，减加速时间T3，加减速时间T5，匀减速时间T6，减减速时间T7为：

步骤(3)、在得出各个时间段的初始值后，可以得到对应加加速、匀加速、减加速、匀速、加减速、匀减速、减减速阶段点的速度公式为(若要求各阶段内某一时刻的速度只需将对应时间改为电机运行时间t即可)：

v₂＝v₁+Jerk*T₁*T₂ T₁≤t＜T₂ 公式(6)

v₄＝v₃ T₃≤t＜T₄ 公式(8)

v₆＝v₅-Jerk*T₅*T₆ T₅≤t＜T₆ 公式(10)

步骤(4)、在求出各阶段时间后，可以得到在理想状态下不包括匀速阶段的位移S为：

步骤(5)、将不包括匀速阶段的位移S与实际所需要运动的距离进行比较，判断是否存在匀速阶段。具体情况可分为①完整的七段S型速度曲线；②不含匀速段的S型速度曲线；③不含匀速段和匀减速段的S型速度曲线；④不含匀减速段的S型速度曲线；⑤基本四段速度曲线；⑥不含匀加减速的S型速度曲线；⑦不含匀加速和匀速段的S型速度曲线。

步骤(6)、在规划好一条加减速曲线后，以倍频于插补周期的频率对指定内存进行采样，若发现由CAN总线传输至内存的终点坐标与上一周期的坐标差值大于阈值，则认为发生了终点位置的突变，需要将该时刻所处位置、运行速度以及终点坐标重新代入计算，得到新一轮的速度规划曲线。

步骤(7)、在确定好一个运动周期内所需要达到的速度后，DSP核可以通过一个周期的时间和对应的速度得到该周期所需运动的步长，对应步骤为：首先要将空间曲线投影至三轴，得到三个电机需要移动的最终位移，再根据指令中的进给速度F(mm/min)计算出每一周期的轮廓步长ΔL，进而计算出各坐标轴进给量Δx和Δy，控制x和y坐标轴分别以速度为和进给，即可到达下一插补点，在每次插补判断是否到达终点前后我们都要对由来自于相机的坐标信号进行采样，分析判断插补终点是否改变，采样频率约为插补频率的3倍左右。若分析得终点坐标发生改变，则调整插补算法中的终点，若无变动，则以相同终点继续插补不做任何改变。

在时间分割直线插补法中以(X_e,Y_e)为终点(本文为传输至电机控制芯片的落点坐标)，则可得到两组推导公式：

其插补示例如图2所示，假设起点为P_s(X_s,Y_s,Y_s),终点为P_e(X_e, Y _e，Z_e)，插补周期为t(ms)，采样周期与插补周期相同，速度为v(mm/ms)，可得：

①程序段各坐标轴增量的计算为：

ΔX＝X_e-X_s 公式(14)

ΔY＝Y_e-Y_s 公式(15)

ΔZ＝Z_e-Z_s 公式(16)

②程序段直线长度的计算为：

③插补进给量的计算为：

④程序段各坐标轴步长的计算为：

s_LX＝f×δX 公式(19)

s_LY＝f×δY 公式(20)

s_LZ＝f×δZ 公式(21)

⑤初始化终点判断标识逻辑变量：

若X_e＝X_s，则f_x＝1；否则f_x＝0

若Y_e＝Y_s，则f_y＝1；否则f_y＝0

若Z_e＝Z_s，则f_z＝1；否则f_z＝1

⑥初始化终点判断变量：

如果X_e≥X_s，则S_x＝1；否则S_x＝-1

如果Y_e≥Y_s，则S_y＝1；否则S_y＝-1

如果Z_e≥Z_s，则S_z＝1；否则S_z＝-1

⑦分速度：

V_x＝V×sinα 公式(22)

⑧速度比：

步骤(8)、在确定了脉冲频率后，DSP核在一个周期内发送对应脉冲个数，实际发送的脉冲个数由ARM核进行计数，实际移动的脉冲个数由精度为17位的光电编码器进行计数并反馈，从而得到在未达到对应进给速度时与其的差值。在普通的PID反馈控制中，输入因子和输出的量化因子均是固定的，因此难以满足较大范围的控制需求，我们考虑到要在保持快速性和稳定性的基础上扩大可控范围的需求，设计了将系统误差e和误差的变化量Δe作为输入因子的非线性模糊化控制器，同时采用了以查表方式获得控制指令，进一步提高了处理速度，最后将经过了模糊规则处理后的K_p,K_i,K_d三个参数输入普通

PID控制器，其中模糊PID控制器的结构设计如图3所示。

在控制规则中，偏差e取值为“PB”,“PM”,“PS”,“ZO”,“NS”,“NM”,“NB”七值，Δe取值范围与e相同，根据蕴含式if e＝A_i and Δe＝B_i then K_p＝C_i,K_d＝D_i可得以下模糊控制规则表，如表1所示：

表1 模糊控制规则表

Tab.1 Table of fuzzy control rules

其中g₁(e)＝1,g₂(e)＝0.7,g₃(e)＝0.35。

经研究验证我们发现，在模糊自适应机构中，U_f＝f(e,Δe)K_ue，f的值可通过查模糊控制规则表确定，再结合传统PID算法离散表达式为:

T_s为采样周期，因此模糊自适应PID输出为：

其中T_i,T_d分别为积分时间常量和微分时间常量。

对于比例因子，本文设置为根据e及Δe进行自适应参数修正以达到提升PID控制器性能的作用，其修正规则为：若e及Δe较大时，应取较小的K_e和K_Δe值以及较大的K_d和K_i值，从而达到降低对偏差以及偏差变化量的分辨率，加快响应速度的目的；当系统接近稳态后，则需要取较大的K_e和K_Δe值和较小的K_d和K_i值，从而减小系统的超调现象，当误差小于阈值后，可转而采用传统PID算法对误差进行进一步微调。根据以上修正模块，可以由e及Δe的值推理出参数修正的倍数n，从而计算得到新的参数进行控制，积分时间常量又可以由微分时间常量推理得出：

T_i＝aT_d 公式(27)

得到积分时间常量后可得到：

最后将U_f和式(27),(28)代入式(26)后可以得到如下输出式:

在通过采样发现脉冲频率发生改变后，则需要重新计算e及Δe，从而初始化模糊PID控制器，达到自适应的效果。

以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims

1.用于直角坐标机器人在高速运动中实现在线多级调整方法，其特征在于：在传统CNC系统中以非对称、多内核的控制器为基础，增加了运动控制算法优化模块，实现了以自适应的速度曲线规划、基于变速的插补控制及自适应的模糊PID反馈控制相结合的控制方法。

2.根据权利要求1所述的用于直角坐标机器人在高速运动中实现在线多级调整方法，其特征在于：所述的非对称、多内核的控制器，划分一块与工控主机通信所专用的内存空间，用于存放实时更新的终点位置坐标；同时采用了以IPC通信机制、MSGRAM为基础，结合了中断情况判断、划分数据修改请求动态优先级的拥有时效性保证的变量共享机制。

3.根据权利要求1所述的用于直角坐标机器人在高速运动中实现在线多级调整方法，其特征在于：所述的运动控制算法优化模块，用于获取实时的运动位置坐标及进行速度曲线规划，进行插补运动并实现反馈控制。

4.根据权利要求1所述的用于直角坐标机器人在高速运动中实现在线多级调整方法，其特征在于：所述的自适应的速度曲线规划控制，通过倍频采样的方式从指定内存中获取实时的运动终点位置坐标，从而通过经优化后的速度规划方法，得到自适应的、实时的速度变化曲线；一旦发现两次采样得到的终点坐标差值大于阈值，则认为发生了终点变化，需要实时更新速度曲线。

5.根据权利要求4所述的用于直角坐标机器人在高速运动中实现在线多级调整方法，其特征在于：所述的优化后的速度规划方法，具体包括以下步骤：

v₂＝v₁+Jerk*T₁*T₂ T₁≤t＜T₂ 公式(6)

v₄＝v₃ T₃≤t＜T₄ 公式(8)

v₆＝v₅-Jerk*T₅*T₆ T₅≤t＜T₆ 公式(10)

6.根据权利要求1所述的用于直角坐标机器人在高速运动中实现在线多级调整方法，其特征在于：所述的自适应的模糊PID控制，引入了周期性采样的环节，保证了当发生速度改变时，可以较快地将其引入模糊PID控制器中进行运算。