用于消除因机械手的抖动而产生的位置偏差的系统和方法
【专利摘要】本发明属于控制系统和控制方法【技术领域】，特别涉及一种用于消除因机械手的抖动而产生的位置偏差的系统，包括主控制模块、通信模块、伺服脉冲输出模块、伺服反馈脉冲捕获模块和控制信号输入输出模块，主控制模块包括偏差脉冲保存模块、偏差脉冲消除模块、数模转换模块、偏差脉冲转换模块和模数转换模块。相对于现有技术，本发明通过设置偏差脉冲保存模块、偏差脉冲消除模块和偏差脉冲转换模块，可以去掉机械手停在B位置时捕获到的位置偏差量，进而消除由于机械手的抖动所产生的位置偏差，机械手就可以正确地回到A位置（原位置），或者准确地移动到其他位置，同时也消除了机械手多次移动过程中的偏差累加的效应。
【专利说明】用于消除因机械手的抖动而产生的位置偏差的系统和方法
【技术领域】
[0001] 本发明属于控制系统和控制方法【技术领域】，特别涉及一种用于消除因机械手(特 别是单轴伺服机械手）的抖动而产生的位置偏差的系统和方法。
【背景技术】
[0002] 现有技术中的单轴伺服机械手系统如图1所示，其包括机械手10、与机械手10连 接的伺服电机20和与伺服电机20电连接的控制系统30,伺服电机20在控制系统30的控 制下对机械手10进行有序的驱动，同时伺服电机20会将其对应的信号反馈给控制系统30， 控制系统30可以通过伺服电机20所反馈的信号来判断机械手10运行过程中产生的偏差。
[0003] 在理想的情况下，伺服电机20根据控制系统30给出的脉冲信号对机械手10进行 驱动，同时将其对应的脉冲信号反馈给控制系统30,就不会出现位置偏差。但是，若出现下 述情况，就会出现机械手10的位置偏差：当控制系统30通过伺服电机20让机械手10的Z 轴从A位置运行到B位置后，机械手10停在B位置处理其他事件，此时若机械手10出现抖 动，就会使机械手10的位置从B位置移动到C位置，当控制系统30再通过伺服电机20让 机械手10的Z轴回到A位置的时候，实际上机械手10并没有回到A位置，而是相对于A位 置有了一定的偏差。如果机械手10的Z轴不停地做A位置和B位置之间的往复运动，则由 于抖动产生的偏差会一直存在，还会产生偏差累加的效应！ 有鉴于此，确有必要提供一种用于消除因机械手的抖动而产生的位置偏差的系统和方 法，以消除由于机械手的抖动所产生的位置偏差，同时也消除了机械手的多次移动过程中 的偏差累加的效应。
【发明内容】
[0004] 本发明的目的之一在于：针对现有技术的不足，而提供一种用于消除因机械手的 抖动而产生的位置偏差的系统，以消除由于机械手的抖动所产生的位置偏差，同时也消除 了机械手的多次移动过程中的偏差累加的效应。
[0005] 为了实现上述目的，本发明所采用如下技术方案： 用于消除因机械手的抖动而产生的位置偏差的系统，包括主控制模块、通信模块和分 别与同机械手连接的伺服电机电连接的伺服脉冲输出模块、伺服反馈脉冲捕获模块和控制 信号输入输出模块，所述主控制模块与所述通信模块双向通信，所述伺服反馈脉冲捕获模 块的输出端与所述主控制模块的输入端连接，所述伺服脉冲输出模块和所述控制信号输入 输出模块的输入端均与所述主控制模块的输出端连接，所述主控制模块包括偏差脉冲保存 模块、偏差脉冲消除模块、数模转换模块、偏差脉冲转换模块和模数转换模块，所述伺服反 馈脉冲捕获模块的输出端与所述偏差脉冲保存模块的输入端连接，所述偏差脉冲保存模块 的输出端与所述偏差脉冲转换模块的输入端连接，所述偏差脉冲转换模块的输出端和所述 数模转换模块的输出端均与所述偏差脉冲消除模块的输入端连接，所述偏差脉冲消除模块 的输出端与所述伺服脉冲输出模块的输入端连接，所述模数转换模块的输入端和输出端分 别与所述伺服反馈脉冲捕获模块的输出端和所述通信模块的输入端连接，所述数模转换模 块的输入端和输出端分别与所述通信模块的输出端和所述伺服脉冲输出模块的输入端连 接。
[0006] 作为本发明用于消除因机械手的抖动而产生的位置偏差的系统的一种改进，所述 通信模块包括解码模块。
[0007] 由图1可以看到：当机械手停在B位置处理其他事件，此时若机械手出现抖动，而 使得机械手的位置从B位置移动到C位置时，此时，实际上是机械手反驱动了伺服电机运 行，伺服电机运行后同时输出反馈脉冲信号给控制系统，如果这个时候控制系统对伺服电 机的反馈脉冲进行捕获并保存下来，等到机械手要从B位置回到A位置的时候，只要在控制 系统输出脉冲信号给伺服电机的时候去掉机械手停在B位置时捕获到的位置偏差量，进而 消除由于机械手的抖动所产生的位置偏差，机械手就可以正确地回到A位置(原位置)，或者 准确地移动到其他位置，同时也消除了机械手的多次移动过程中的偏差累加的效应。
[0008] 本发明正是基于这种分析，通过设置偏差脉冲保存模块、偏差脉冲消除模块、和偏 差脉冲转换模块，当机械手停在B位置处理其他事件，此时若机械手出现抖动，而使得机械 手的位置从B位置移动到C位置时，本发明的偏差脉冲保存模块可以保存偏差脉冲，而偏差 脉冲转换模块则能将该偏差脉冲转换成对应的伺服输出脉冲数量，当通信模块接收到下一 个伺服动作指令并传输给主控制模块进行数模转换后，偏差脉冲消除模块就可以去掉机械 手停在B位置时捕获到的位置偏差量，进而消除由于机械手的抖动所产生的位置偏差，机 械手就可以正确地回到A位置(原位置)，或者准确地移动到其他位置，同时也消除了机械手 的多次移动过程中的偏差累加的效应。
[0009] 本发明的另一个目的在于提供一种采用本发明所述的系统消除因机械手的抖动 而产生的位置偏差的方法，包括以下步骤： S1，所述通信模块接收伺服动作指令，并将该指令发送给所述主控制模块，所述主控制 模块的数模转换模块把伺服动作指令中的位置量转换成对应的脉冲数量，再由所述伺服脉 冲输出模块输出给所述伺服电机； 52, 所述伺服电机开始工作，带动机械手移动，同时所述伺服反馈脉冲捕获模块开始对 反馈脉冲进行捕获，并把捕获到的脉冲数量发送到所述主控制模块，所述主控制模块的模 数转换模块将捕获到的脉冲数量转换成位置量，再通过所述通信模块发送出去； 53, 当没有伺服脉冲输出的时候，若所述伺服反馈脉冲捕获模块捕获到了反馈脉冲，这 说明机械手出现了抖动，所述伺服反馈脉冲捕获模块就将该反馈脉冲的脉冲数量发送给所 述主控制模块，并保存于所述偏差脉冲保存模块，经过所述偏差脉冲转换模块后，该反馈脉 冲的脉冲数量转换成对应的伺服输出脉冲数量Ml ; 54, 所述主控制模块在接收到下一次伺服动作指令的时候，所述主控制模块的数模转 换模块把伺服动作指令中的位置量转换成对应的脉冲数量M2,经过所述偏差脉冲消除模块 时，所述偏差脉冲消除模块通过（M1+M2)、（M1-M2)或（M2-M1)的运算处理实现位置偏差的 消除，之后再由所述伺服脉冲输出模块输出给所述伺服电机。
[0010] 作为本发明消除因机械手的抖动而产生的位置偏差的方法的一种改进，S1中，所 述通信模块接收伺服动作指令后，还通过解码模块对其接收到的伺服动作指令进行解码操 作。 toon] 相对于现有技术，本发明能够捕获并保存偏差脉冲信号，并将其转换成对应的伺 服输出脉冲数量，当通信模块接收到下一个伺服动作指令并传输给主控制模块进行数模转 换后，偏差脉冲消除模块就可以去掉机械手停在B位置时捕获到的位置偏差量，进而消除 由于机械手的抖动所产生的位置偏差，机械手就可以正确地回到A位置(原位置)，或者准确 地移动到其他位置，同时也消除了机械手的多次移动过程中的偏差累加的效应。
【专利附图】
【附图说明】
[0012] 图1为现有技术中的单轴伺服机械手系统的结构示意图。
[0013] 图2为本发明的系统架构示意图。
[0014] 图3为本发明的方法的流程框图。
【具体实施方式】
[0015] 下面结合实施例和说明书附图，对本发明及其有益效果进行进一步详细的说明， 但本发明的实施方式不限于此。
[0016] 如图2所示，本发明提供的一种用于消除因单轴伺服机械手的抖动而产生的位置 偏差的系统，包括主控制模块1、通信模块2和分别与同单轴伺服机械手连接的伺服电机电 连接的伺服脉冲输出模块3、伺服反馈脉冲捕获模块4和控制信号输入输出模块5 (即伺服 脉冲输出模块3、伺服反馈脉冲捕获模块4和控制信号输入输出模块5均与伺服电机连接， 而伺服电机则与单轴伺服机械手连接，用于控制单轴伺服机械手的动作)，主控制模块1与 通信模块2双向通信，伺服反馈脉冲捕获模块4的输出端与主控制模块1的输入端连接，伺 服脉冲输出模块2和控制信号输入输出模块5的输入端均与主控制模块1的输出端连接， 主控制模块1包括偏差脉冲保存模块11、偏差脉冲消除模块12、数模转换模块13、偏差脉冲 转换模块14和模数转换模块15,伺服反馈脉冲捕获模块4的输出端与偏差脉冲保存模块 11的输入端连接，偏差脉冲保存模块11的输出端与偏差脉冲转换模块14的输入端连接，偏 差脉冲转换模块14的输出端和数模转换模块15的输出端均与偏差脉冲消除模块12的输 入端连接，偏差脉冲消除模块12的输出端与伺服脉冲输出模块3的输入端连接，模数转换 模块15的输入端和输出端分别与伺服反馈脉冲捕获模块4的输出端和通信模块2的输入 端连接，数模转换模块13的输入端和输出端分别与通信模块2的输出端和伺服脉冲输出模 块3的输入端连接。
[0017] 其中，通信模块2包括解码模块21。
[0018] 采用该系统可以消除因单轴伺服机械手的抖动而产生的位置偏差，如图3所示， 其具体包括如下步骤： S1，通信模块2接收伺服动作指令后，通过解码模块21对其接收到的伺服动作指令进 行解码操作，然后将该指令发送给主控制模块1，主控制模块1的数模转换模块13把伺服动 作指令中的位置量转换成对应的脉冲数量，再由伺服脉冲输出模块3输出给伺服电机； 52, 伺服电机开始工作，带动单轴伺服机械手沿着Z轴移动，同时伺服反馈脉冲捕获模 块4开始对反馈脉冲进行捕获，并把捕获到的脉冲数量发送到主控制模块1，主控制模块1 的模数转换模15块将捕获到的脉冲数量转换成位置量，再通过通信模块2发送出去； 53, 当没有伺服脉冲输出的时候（即机械手停止在Z轴上移动后)，若伺服反馈脉冲捕获 模块4捕获到了反馈脉冲，则说明单轴伺服机械手出现了抖动，伺服反馈脉冲捕获模块4就 将该反馈脉冲的脉冲数量发送给主控制模块1，并保存于偏差脉冲保存模块11，经过偏差 脉冲转换模块14后，该反馈脉冲的脉冲数量转换成对应的伺服输出脉冲数量Ml ;若伺服反 馈脉冲捕获模块4未捕获到反馈脉冲，则一次动作结束。
[0019] S4,主控制模块1在接收到下一次伺服动作指令的时候，主控制模块1的数模转换 模块13把伺服动作指令中的位置量转换成对应的脉冲数量M2,经过偏差脉冲消除模块12 时，偏差脉冲消除模块12通过（M1+M2)、（M1-M2)或或（M2-M1)的运算处理实现位置偏差的 消除，之后再由伺服脉冲输出模块2输出给伺服电机。
[0020] 具体而言，当单轴伺服机械手的Z轴从A位置运行到B位置后，若在单轴伺服机械 手停在B位置处理其他事件的时候，单轴伺服机械手出现抖动，而使得单轴伺服机械手的 位置从B位置移动到C位置时，若C位置到A位置的距离大于B位置到A位置的距离，当主控 制模块1在接收到让机械手回到A位置的指令的时候，偏差脉冲消除模块12通过(M1+M2) 的运算处理可实现位置偏差的消除，使得单轴伺服机械手能够从C位置准确地回到A位置。 相反，若C位置到A位置的距离小于B位置到A位置的距离，当主控制模块1在接收到让机 械手回到A位置的指令的时候，偏差脉冲消除模块12通过（M1-M2)的运算处理可实现位置 偏差的消除，使得单轴伺服机械手能够从C位置准确地回到A位置。
[0021] 当然，若主控制模块1接收到的下一次伺服动作指令并不是回到A位置，而是移到 另一个位置D时，分为以下几种情况： (1)若C位置到A位置的距离大于B位置到A位置的距离，且若D位置到A位置的距离 大于C位置到A位置的距离时，偏差脉冲消除模块12通过（M1-M2)的运算处理可实现位置 偏差的消除，使得单轴伺服机械手能够从C位置准确地移动到D位置。
[0022] (2)若C位置到A位置的距离大于B位置到A位置的距离，且若D位置到A位置的 距离小于C位置到A位置的距离且大于B位置到A位置的距离时，偏差脉冲消除模块12通 过(M2-M1)的运算处理可实现位置偏差的消除，使得单轴伺服机械手能够从C位置准确地 移动到D位置。
[0023] (3)若C位置到A位置的距离大于B位置到A位置的距离，且若D位置到A位置的 距离小于B位置到A位置的距离时，偏差脉冲消除模块12通过（M2+M1)的运算处理可实现 位置偏差的消除，使得单轴伺服机械手能够从C位置准确地移动到D位置。
[0024] (4)若C位置到A位置的距离小于B位置到A位置的距离，且若D位置到A位置大 于B位置到A位置的距离时，偏差脉冲消除模块12通过（M2+M1)的运算处理可实现位置偏 差的消除，使得单轴伺服机械手能够从C位置准确地移动到D位置。
[0025] (5)若C位置到A位置的距离小于B位置到A位置的距离，且若D位置到A位置的 距离大于C位置到A位置的距离且小于B位置到A位置的距离时，偏差脉冲消除模块12通 过(M1-M2)的运算处理可实现位置偏差的消除，使得单轴伺服机械手能够从C位置准确地 移动到D位置。
[0026] (6)若C位置到A位置的距离小于B位置到A位置的距离，且若D位置到A位置的 距离小于C位置到A位置的距离时，偏差脉冲消除模块12通过（M2-M1)的运算处理可实现 位置偏差的消除，使得单轴伺服机械手能够从C位置准确地移动到D位置。
[0027] 总之，本发明能够捕获并保存偏差脉冲信号，并将其转换成对应的伺服输出脉冲 数量，当通信模块2接收到下一个伺服动作指令并传输给主控制模块1进行数模转换后，偏 差脉冲消除模块12就可以去掉单轴伺服机械手停在B位置时捕获到的位置偏差量，进而消 除由于机械手的抖动所产生的位置偏差，机械手就可以正确地回到A位置(原位置)，或者准 确地移动到其他位置，同时也消除了机械手的多次移动过程中的偏差累加的效应。
[0028] 根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方 式进行变更和修改。因此，本发明并不局限于上面揭示和描述的【具体实施方式】，对本发明的 一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用 了一些特定的术语，但这些术语只是为了方便说明，并不对本发明构成任何限制。
【权利要求】
1. 用于消除因机械手的抖动而产生的位置偏差的系统，包括主控制模块、通信模块和 分别与同机械手连接的伺服电机电连接的伺服脉冲输出模块、伺服反馈脉冲捕获模块和控 制信号输入输出模块，所述主控制模块与所述通信模块双向通信，所述伺服反馈脉冲捕获 模块的输出端与所述主控制模块的输入端连接，所述伺服脉冲输出模块和所述控制信号输 入输出模块的输入端均与所述主控制模块的输出端连接，其特征在于：所述主控制模块包 括偏差脉冲保存模块、偏差脉冲消除模块、数模转换模块、偏差脉冲转换模块和模数转换模 块，所述伺服反馈脉冲捕获模块的输出端与所述偏差脉冲保存模块的输入端连接，所述偏 差脉冲保存模块的输出端与所述偏差脉冲转换模块的输入端连接，所述偏差脉冲转换模块 的输出端和所述数模转换模块的输出端均与所述偏差脉冲消除模块的输入端连接，所述偏 差脉冲消除模块的输出端与所述伺服脉冲输出模块的输入端连接，所述模数转换模块的输 入端和输出端分别与所述伺服反馈脉冲捕获模块的输出端和所述通信模块的输入端连接， 所述数模转换模块的输入端和输出端分别与所述通信模块的输出端和所述伺服脉冲输出 模块的输入端连接。
2. 根据权利要求1所述的用于消除因机械手的抖动而产生的位置偏差的系统，其特征 在于：所述通信模块包括解码模块。
3. -种采用权利要求1所述的系统消除因机械手的抖动而产生的位置偏差的方法，其 特征在于，包括以下步骤： S1，所述通信模块接收伺服动作指令，并将该指令发送给所述主控制模块，所述主控制 模块的数模转换模块把伺服动作指令中的位置量转换成对应的脉冲数量，再由所述伺服脉 冲输出模块输出给所述伺服电机； 52, 所述伺服电机开始工作，带动机械手移动，同时所述伺服反馈脉冲捕获模块开始对 反馈脉冲进行捕获，并把捕获到的脉冲数量发送到所述主控制模块，所述主控制模块的模 数转换模块将捕获到的脉冲数量转换成位置量，再通过所述通信模块发送出去； 53, 当没有伺服脉冲输出的时候，若所述伺服反馈脉冲捕获模块捕获到了反馈脉冲，所 述伺服反馈脉冲捕获模块就将该反馈脉冲的脉冲数量发送给所述主控制模块，并保存于所 述偏差脉冲保存模块，经过所述偏差脉冲转换模块后，该反馈脉冲的脉冲数量转换成对应 的伺服输出脉冲数量Ml ; 54, 所述主控制模块在接收到下一次伺服动作指令的时候，所述主控制模块的数模转 换模块把伺服动作指令中的位置量转换成对应的脉冲数量M2,经过所述偏差脉冲消除模块 时，所述偏差脉冲消除模块通过（M1+M2)、（M1-M2)或（M2-M1)的运算处理实现位置偏差的 消除，之后再由所述伺服脉冲输出模块输出给所述伺服电机。
4. 根据权利要求3所述的消除因机械手的抖动而产生的位置偏差的方法，其特征在 于：S1中，所述通信模块接收伺服动作指令后，还通过解码模块对其接收到的伺服动作指 令进行解码操作。
【文档编号】B25J9/18GK104057451SQ201410275881
【公开日】2014年9月24日 申请日期:2014年6月19日 优先权日:2014年6月19日
【发明者】陈雄辉 申请人:东莞市菲帆自动化科技有限公司