一种转臂式清罐装置万向驱动机构控制方法
【专利摘要】本发明公开了一种转臂式清罐装置万向驱动机构控制方法，该方法所使用的装置包括控制器、两个角度传感器、第一回转臂驱动装置，转向电机，万向驱动机构。控制器依据角度传感器采样的第一回转臂的关节转角θ1′，第二回转臂的关节转角θ2′，第一回转臂的关节速度第二回转臂的关节速度计算万向驱动机构转向调整角度θ，控制转向电机调整万向驱动机构使其主动驱动轮的运动方向与其安装处第二回转臂B点的合成线速度vB方向一致。本发明采用主动转向的方法使转向运动与驱动运动同步进行，实时调整驱动机构运动方向与第二回转臂B点处的合成线速度vB方向一致，保证转臂式清罐装置回转顺畅，降低换热管定位时间，正确定位清洗的换热管。
【专利说明】
一种转臂式清罐装置万向驱动机构控制方法
技术领域
[0001] 本发明涉及清洗设备和位置或方向的控制领域的方向控制方法，特别是一种转臂 式清罐装置万向驱动机构控制方法。 技术背景
[0002] 列管式蒸发罐的换热管内壁结垢问题是影响制糖业制糖效率与节能降耗的一大 问题，通常，这种蒸发罐直径大小不一（直径约4-8米），其内换热管工作面分布的换热管 数量巨大而且分布不规则。为替代目前国内各糖厂实际普遍采用的人工高压水逐根清洗换 热管，国内外学者对蒸发罐清洗装置进行了大量研究。中国专利文献号CN 102305574 B于 2011年8月23日公开的蒸发器换热管清洗机，该装置通过清洗机头和清洗机架的配合将清 洗水枪输送到换热管中，自动定位清洗换热管，一方面由于其机械结构的限制，其工作的固 定轨道与蒸发罐最外沿的换热管重叠，存在清洗盲区；另方面由于其采用固定机架，仅适用 于直径大小与机架长度相匹配的蒸发罐的清洗；国专利文献号CN 103272813 A于2013年 9月4日公开的列管式加热器全自动清洗设备采用X、Y轴滑轨分区、换区方式自动定位清 洗换热管，其在不同区中执行清洗任务时，需要重复定位原点，影响设备的定位精度，由于 采用固定机架，其亦不能匹配直径大小不同的蒸发罐的清洗；国专利文献号CN 104006703 A于2014年8月27日公开的一种清罐设备，由于其采用的两级转臂式结构的第二回转臂为 悬臂梁结构，刚性差，承重能力小，回转支座端弯矩大，在清洗过程中易受力而引起清罐设 备机械本体发生弯曲变形造成清洗过程第二回转臂的上下晃动，进而对换热管的清洗产生 影响。同时，其第二回转臂驱动装置安装于第二回转臂回转中心处，为减轻驱动装置驱动电 机端的负载需配合多级减速器满足大减速比的要求，大减速比的减速器结构尺寸庞大，不 紧凑。
【发明内容】
[0003] 针对悬臂梁结构及第二回转臂驱动装置安装于第二回转臂回转中心处的问题，提 供一种非悬臂的转臂式清罐装置，其第二回转臂万向驱动机构安装在第二回转臂末端构成 的简支梁结构有效解决悬臂梁结构引起的问题，同时，驱动机构安装在第二回转臂末端的 主动驱动方式有效降低电机端的负载，有效降低配合减速器减速比，驱动装置结构紧凑。本 发明的目的在于提供一种蒸发罐清洗设备万向驱动机构的转向控制方法，采用主动转向的 方法使转向运动与驱动运动同步进行，实时调整驱动机构使其主动驱动轮的运动方向与第 二回转臂B点处的合成线速度％方向一致，保证转臂式清罐装置回转顺畅，降低换热管定 位时间，正确定位清洗的换热管。
[0004] 为了实现上述目标，本发明的技术方案如下：
[0005] -种转臂式清罐装置万向驱动机构控制方法，该方法所使用的装置包括控制器1、 角度传感器2、角度传感器4、第一回转臂驱动装置5,转向电机7,万向驱动机构8 ;角度传 感器2安装在第一回转臂3的轴线1上，传感器中心与轴线1重合，用于检测第一回转臂3 的关节转角θ' i，角度传感器4安装在第二回转臂6的轴线2上，传感器中心与轴线2重 合，用于检测第二回转臂6的关节转角θ' 2，转向电机7安装在第二回转臂6轴线3处， 转向电机7依据第一回转臂3的关节转角θ' i，第二回转臂6的关节转角θ' 2实时调 整万向驱动机构8使其主动驱动轮的运动方向与第二回转臂6 Β点处的合成线速度％方 向一致。
[0006] 所述的一种转臂式清罐装置万向机构控制方法的具体过程为：
[0007] 控制器1根据给定的第一回转臂3的目标关节转角Θ i (以第一回转臂3回转轴线 1与回转平面的交点〇为原点建立直角坐标系），第二回转臂6的目标关节转角Θ 2，第一回 转臂3的关节速度^，第二回转臂6的关节速度^，分别给第一回转臂驱动装置5,万向驱动 机构8发送指令脉冲，第一回转臂驱动装置5根据所述指令脉冲驱动第一回转臂3绕轴线1 回转运动，第一回转臂3的关节转角θ' i由角度传感器2检测并反馈给控制器1;同时万 向驱动机构8根据所述指令脉冲驱动第二回转臂6绕轴线2回转运动，第二回转臂6的关 节转角 2由角度传感器4检测并反馈给控制器1 ;控制器1按照给定的角度控制算法， 依据第一回转臂3的关节转角θ' i，第二回转臂6的关节转角θ' 2，第一回转臂3的关 节速度&，第二回转臂6的关节速度^计算万向驱动机构8转向调整角度Θ，控制器1根 据调整角度Θ控制转向电机7调整驱动机构8使其主动驱动轮的运动方向与第二回转臂 6Β点处的合成线速度νΒ方向一致。
[0008] 所述的角度控制算法，包括以下步骤：
[0009] (1)控制器1参数初始化操作：
[0010] 所述的参数包括第一回转臂杆长Li、第二回转臂杆长L2，插补加速度，插补减速 度，插补速度；
[0011] (2)控制器1接收目标位置参数及关节速度：
[0012] 所述的目标位置参数包括第一回转臂3的目标关节转角Θ i，第二回转臂6的目标 关节转角Θ 2;所述的关节速度包括第一回转臂3的关节速度&，第二回转臂6的关节速度
[0013] (3)判断定位运行是否完成：
[0014] 所述的定位运行完成的判断条件是：第一回转臂3的关节转角Θ ' 1=目标关节 转角Θ i且第二回转臂6的关节转角Θ ' 2=目标关节转角Θ 2;
[0015] 当判断条件成立时，第一回转臂3、第二回转臂6定位到目标位置，角度调整结束； 当判断条件不成立时，则执行下面步骤（4)到步骤（7);
[0016] (4)采样插补点处关节转角参数，所述的关节转角参数包括第一回转臂3的关节 转角θ' i，第二回转臂6的关节转角θ' 2;
[0017] (5)计算插补点位置参数：依据关节转角参数θ ' ρ Θ ' 2计算插补点处第一回 转臂3关节转角位置ΘΗ，第二回转臂6关节转角位置θ 2ι;
[0018] (6)依据第一回转臂3关节转角位置ΘΗ，第二回转臂6关节转角位置θ2ι按公式 (1)计算雅克比矩阵J ;按公式⑵计算Β点处的X方向速度分量vBx，y方向速度分量vBy; 按公式（3)式计算B点处速度vB的方向θ vB，按公式（4)计算调整角度Θ ;
[0022] θ = θνΒ-θΒ〇 (4)
[0023] 其中：θ Β。为万向驱动机构8的初始方向角。
[0024] (7)转向电机7根据调整角度Θ控制调整驱动机构8使其主动驱动轮的运动方向 与第二回转臂6 Β点处的合成线速度％方向一致，返回到步骤（3)。
[0025] 所述的第二回转臂6Β点处不限于第二回转臂6的末端，Β点位置可以依据需要在 第二回转臂6上进行位置调整。
[0026] 所述的角度传感器2安装位置不限于第一回转臂3的轴线1上，还可以安装与轴 线1偏离的一定距离的偏心处，传感器中心与偏心处重合用于检测第一回转臂3的关节转 角θ' 1;所述的角度传感器4安装位置不限于第二回转臂6的轴线2上，还可以安装与轴 线2偏离一定距离的偏心处，传感器中心与偏心处重合用于检测第二回转臂6的关节转角 Θ，2。
[0027] 本发明的有益效果是：
[0028] (1)采用主动式转向方法，依据转臂的关节转角位置实时调整驱动机构使其主动 驱动轮的运动方向与其安装Β点处的合成线速度％方向一致，保证转臂式清罐装置回转顺 畅。
[0029] (2)转向运动与驱动运动同步进行，降低换热管定位时间，提高换热管定位精度。
【附图说明】
[0030] 图1为本发明一种转臂式清罐装置万向驱动机构控制方法使用装置示意图
[0031] 图2为本发明一种转臂式清罐装置万向驱动机构控制方法同向角度调整图。
[0032] 图3为本发明一种转臂式清罐装置万向驱动机构控制方法异向角度调整图。
[0033] 图4为本发明一种转臂式清罐装置万向驱动机构控制方法的角度控制算法流程 图。
[0034] 附图中：1_控制器；2-角度传感器；3-第一回转臂；4-角度传感器；5-第一回转 臂驱动装置；6-第二回转臂；7-转向电机；8-万向驱动机构。
【具体实施方式】
[0035] 下面结合附图对本发明做进一步详细的说明：
[0036] 如图1所示，一种转臂式清罐装置万向驱动机构控制方法，该方法所使用的装置 包括控制器1可以采用不同厂家的已有型号的独立式或PCI卡槽式多轴运动控制器，例如 雷赛PMC6496运动控制器或深圳正运动ZMC206运动控制器，角度传感器2、角度传感器4 可采用不同类型的编码器，例如用海德汉ERN120编码器、多摩川多摩川0IH-48-2500P8,第 一回转臂驱动装置5，例如伺服电机减速器驱动轮或伺服电机减速器履带行走机构，转向电 机7例如伺服电机或步进电机，万向驱动机构8 ;角度传感器2安装在第一回转臂3的轴线 1上，传感器中心与轴线1重合，并检测第一回转臂3的关节转角θ' i，把关节转角θ' i 转换成相对应的脉冲数，角度传感器4安装在第二回转臂6的轴线2上，传感器中心与轴线 2重合，用于检测第二回转臂6的关节转角Θ ' 2并转换成相对应的脉冲数，转向电机7安 装在第二回转臂6轴线3处驱动万向机构8进行转向，控制器1依据第一回转臂3的关节 转角i，第二回转臂6的关节转角Θ ' 2实时计算调整角度Θ控制调整驱动机构8使 其主动驱动轮的运动方向与第二回转臂6 B点处的合成线速度vB方向一致。
[0037] 如图2、图3所示，一种转臂式清罐装置万向机构控制方法至少包括同向角度调 整、异向角度调整两种模式，具体过程为：
[0038] 控制器1根据给定的第一回转臂3的目标关节转角Θ i (以第一回转臂3回转轴线 1与回转平面的交点〇为原点建立直角坐标系），第二回转臂6的目标关节转角Θ 2，第一回 转臂3的关节速度&，第二回转臂6的关节速度，分别给第一回转臂驱动装置5,万向驱动 机构8发送指令脉冲，第一回转臂驱动装置5根据所述指令脉冲驱动第一回转臂3绕轴线1 回转运动，第一回转臂3的关节转角θ' i由角度传感器2检测并反馈给控制器1;同时万 向驱动机构8根据所述指令脉冲驱动第二回转臂6绕轴线2回转运动，第二回转臂6的关 节转角 2由角度传感器4检测并反馈给控制器1 ;控制器1按照给定的角度控制算法， 依据第一回转臂3的关节转角θ' i，第二回转臂6的关节转角θ' 2，第一回转臂3的关 节速度&，第二回转臂6的关节速度么计算万向驱动机构8转向调整角度Θ，控制器1根 据调整角度Θ控制转向电机7调整驱动机构8使其主动驱动轮的运动方向与第二回转臂 6 Β点处的合成线速度νΒ方向一致。
[0039] 如图4所示，所述的角度控制算法，包括以下步骤：
[0040] (1)控制器1参数初始化操作：
[0041] 所述的参数包括第一回转臂杆长Li、第二回转臂杆长L2，插补加速度，插补减速 度，插补速度；
[0042] (2)控制器1接收目标位置参数及关节速度：
[0043] 所述的目标位置参数包括第一回转臂3的目标关节转角Θ i，第二回转臂6的目标 关节转角Θ 2;所述的关节速度包括第一回转臂3的关节速度^，第二回转臂6的关节速度 么；
[0044] (3)判断定位运行是否完成：
[0045] 所述的定位运行完成的判断条件是：第一回转臂3的关节转角Θ ' 1=目标关节 转角Θ i且第二回转臂6的关节转角Θ ' 2=目标关节转角Θ 2;
[0046] 当判断条件成立时，第一回转臂3、第二回转臂6定位到目标位置，角度调整结束； 当判断条件不成立时，则执行下面步骤（4)到步骤（7);
[0047] (4)采样插补点处关节转角参数，所述的关节转角参数包括第一回转臂3的关节 转角θ' i，第二回转臂6的关节转角θ' 2;
[0048] (5)计算插补点位置参数：依据关节转角参数θ ' ρ θ ' 2计算插补点处第一回 转臂3关节转角位置ΘΗ，第二回转臂6关节转角位置θ 2ι;
[0049] (6)依据第一回转臂3关节转角位置θ Η，第二回转臂6关节转角位置θ 2ι按公式 (1)计算雅克比矩阵J ;按公式⑵计算Β点处的X方向速度分量vBx，y方向速度分量vBy; 进而按公式（3)式计算B点处速度％的方向θ vB，按公式（4)计算调整角度Θ ;
[0053] θ = θνΒ-θΒ〇 (4)
[0054] 其中：θ Β。为万向驱动机构8的初始方向角，设定万向驱动机构8的初始方向角为 二级臂末上Β点处vBy的方向一致，则θ Β(]= 90° ;
[0055] (7)转向电机7根据调整角度Θ控制调整驱动机构8使其主动驱动轮的运动方向 与第二回转臂6 Β点处的合成线速度％方向一致，返回到步骤（3)。
[0056] 所述的第二回转臂6Β点处不限于第二回转臂6的末端，Β点位置可以依据需要在 第二回转臂6上进行位置调整。
[0057] 所述的角度传感器2安装位置不限于第一回转臂3的轴线1上，还可以安装与轴 线1偏离的一定距离的偏心处，传感器中心与偏心处重合用于检测第一回转臂3的关节转 角θ' 1;所述的角度传感器4安装位置不限于第二回转臂6的轴线2上，还可以安装与轴 线2偏离一定距离的偏心处，传感器中心与偏心处重合用于检测第二回转臂6的关节转角 Θ，2。
[0058] 最后说明的是本发明的一种转臂式清罐装置万向驱动机构控制方法不局限于上 述实施例，还可以做出各种修改、变换和变形。因此，说明书和附图应被认为是说明性的而 非限制性的。凡是依据本发明的技术方案进行修改、修饰或等同变化，而不脱离本发明技术 方案的思想和范围，其均应涵盖在本发明的权利要求范围当中。
【主权项】
1. 一种转臂式清罐装置万向驱动机构控制方法，该方法所使用的装置包括控制器 (1)、角度传感器（2)、角度传感器（4)、第一回转臂驱动装置（5)，转向电机（7)，万向驱动机 构（8);角度传感器（2)安装在第一回转臂（3)的轴线1上，传感器中心与轴线1重合，用 于检测第一回转臂（3)的关节转角Θ ' i，角度传感器（4)安装在第二回转臂（6)的轴线 2上，传感器中心与轴线2重合，用于检测第二回转臂（6)的关节转角Θ ' 2，转向电机（7) 安装在第二回转臂（6)轴线3处驱动万向机构（8)进行转向；其特征在于，转向电机（7)依 据第一回转臂（3)的关节转角Θ/，第二回转臂（6)的关节转角Θ ' 2实时调整万向驱动 机构（8)使其主动驱动轮的运动方向与第二回转臂（6)B点处的合成线速度vB方向一致； 所述的一种转臂式清罐装置万向机构控制方法的具体过程为： 控制器（1)根据给定的第一回转臂（3)的目标关节转角以第一回转臂（3)回转 轴线1与回转平面的交点〇为原点建立直角坐标系），第二回转臂（6)的目标关节转角θ2， 第一回转臂（3)的关节速度&，第二回转臂(6)的关节速度t分别给第一回转臂驱动装置 (5)，万向驱动机构（8)发送指令脉冲，第一回转臂驱动装置（5)根据所述指令脉冲驱动第 一回转臂（3)绕轴线1回转运动，第一回转臂（3)的关节转角Θ /由角度传感器（2)检测 并反馈给控制器（1);同时万向驱动机构（8)根据所述指令脉冲驱动第二回转臂（6)绕轴 线2回转运动，第二回转臂（6)的关节转角Θ ' 2由角度传感器（4)检测并反馈给控制器 (1);控制器（1)按照角度控制算法，依据第一回转臂（3)的关节转角Θ /，第二回转臂（6) 的关节转角Θ ' 2，第一回转臂（3)的关节速度g,，第二回转臂（6)的关节速度么计算万向 驱动机构（8)转向调整角度Θ，控制器（1)根据调整角度Θ控制转向电机（7)调整驱动机 构（8)使其主动驱动轮的运动方向与第二回转臂（6)B点处的合成线速度v B方向一致； 所述的角度控制算法，包括以下步骤： (1) 控制器（1)参数初始化操作： 所述的参数包括第一回转臂杆长L1、第二回转臂杆长L2，插补加速度，插补减速度，插 补速度； (2) 控制器（1)接收目标位置参数及关节速度： 所述的目标位置参数包括第一回转臂（3)的目标关节转角G1，第二回转臂（6)的目标 关节转角θ2;所述的关节速度包括第一回转臂（3)的关节速度^，第二回转臂（6)的关节 速度么； (3) 判断定位运行是否完成： 所述的定位运行完成的判断条件是：第一回转臂（3)的关节转角Θ/ =目标关节转 角Q1且第二回转臂（6)的关节转角Θ 2'=目标关节转角02; 当判断条件成立时，第一回转臂（3)、第二回转臂（6)定位到目标位置，角度调整结束； 当判断条件不成立时，则执行下面步骤（4)到步骤（7); (4) 采样插补点处关节转角参数，所述的关节转角参数包括第一回转臂（3)的关节转 角Θ/，第二回转臂（6)的关节转角θ2'; (5) 计算插补点位置参数：依据关节转角参数θ/，θ2'计算插补点处第一回转臂 (3)关节转角位置Θ n，第二回转臂（6)关节转角位置θ2ι; (6) 依据第一回转臂（3)关节转角位置Θ n，第二回转臂（6)关节转角位置θ 2ι按公式 (1)计算雅克比矩阵J ;按公式⑵计算B点处的X方向速度分量vBx，y方向速度分量vBy; 按公式（3)式计算B点处速度vB的方向θ vB，按公式（4)计算调整角度Θ ; (1)(2) (3) 其中：θ B。为万向驱动机构⑶的初始方向角； (7)转向电机（7)根据调整角度Θ控制调整驱动机构（8)使其主动驱动轮的运动方向 与第二回转臂（6)B点处的合成线速度％方向一致，返回到步骤（3)。2. 根据权利要求1所述的一种转臂式清罐装置万向驱动机构控制方法，其特征在于： 所述的第二回转臂（6)B点处不限于第二回转臂（6)的末端，B点位置可以依据需要在第二 回转臂（6)上进行位置调整。3. 根据权利要求1所述的一种转臂式清罐装置万向驱动机构控制方法，其特征在于： 所述的角度传感器（2)安装位置不限于第一回转臂（3)的轴线1上，还可以安装与轴线1 偏离的一定距离的偏心处，传感器中心与偏心处重合用于检测第一回转臂（3)的关节转角 Θ 所述的角度传感器（4)安装位置不限于第二回转臂（6)的轴线2上，还可以安装与 轴线2偏离一定距离的偏心处，传感器中心与偏心处重合用于检测第二回转臂(6)的关节 转角。
【文档编号】F28G15/00GK105890441SQ201410767167
【公开日】2016年8月24日
【申请日】2014年12月15日
【发明人】潘海鸿, 陈琳, 涂晓晓, 黄炳琼, 王萌, 龚建刚
【申请人】广西大学